From 4f27ddef1348d93ff75ef2d54e772e5cd5c676ab Mon Sep 17 00:00:00 2001 From: klaxalk Date: Wed, 6 Dec 2023 07:59:49 +0000 Subject: [PATCH] deploy: 79907194e76cc2d2102281b28dbb7832f648a174 --- .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 | 672 ++ index-sort-l.html | 672 ++ index.html | 672 ++ .../attitude_converter.h.func-sort-c.html | 128 + .../mrs_lib/attitude_converter.h.func.html | 128 + .../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 -> 1660 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 | 218 + .../mrs_lib/impl/index-detail-sort-l.html | 218 + .../include/mrs_lib/impl/index-detail.html | 218 + .../include/mrs_lib/impl/index-sort-f.html | 162 + .../include/mrs_lib/impl/index-sort-l.html | 162 + mrs_lib/include/mrs_lib/impl/index.html | 162 + .../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 | 856 ++ .../impl/publisher_handler.hpp.func.html | 856 ++ .../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 -> 842 bytes ...ervice_client_handler.hpp.func-sort-c.html | 252 + .../impl/service_client_handler.hpp.func.html | 252 + ...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 | 4024 ++++++++ .../impl/subscribe_handler.hpp.func.html | 4024 ++++++++ .../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 .../include/mrs_lib/index-detail-sort-f.html | 426 + .../include/mrs_lib/index-detail-sort-l.html | 426 + mrs_lib/include/mrs_lib/index-detail.html | 426 + mrs_lib/include/mrs_lib/index-sort-f.html | 282 + mrs_lib/include/mrs_lib/index-sort-l.html | 282 + mrs_lib/include/mrs_lib/index.html | 282 + .../include/mrs_lib/lkf.h.func-sort-c.html | 244 + mrs_lib/include/mrs_lib/lkf.h.func.html | 244 + .../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 -> 1284 bytes .../include/mrs_lib/mutex.h.func-sort-c.html | 404 + mrs_lib/include/mrs_lib/mutex.h.func.html | 404 + .../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 | 368 + .../include/mrs_lib/param_loader.h.func.html | 368 + .../mrs_lib/param_loader.h.gcov.frameset.html | 19 + .../include/mrs_lib/param_loader.h.gcov.html | 1397 +++ .../mrs_lib/param_loader.h.gcov.overview.html | 349 + .../include/mrs_lib/param_loader.h.gcov.png | Bin 0 -> 4285 bytes .../publisher_handler.h.func-sort-c.html | 532 + .../mrs_lib/publisher_handler.h.func.html | 532 + .../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 | 304 + .../include/mrs_lib/repredictor.h.func.html | 304 + .../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 | 168 + .../service_client_handler.h.func.html | 168 + ...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 | 2876 ++++++ .../mrs_lib/subscribe_handler.h.func.html | 2876 ++++++ .../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 -> 1794 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 -> 466 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 .../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 | 182 + .../visual_object.h.gcov.overview.html | 45 + .../include/mrs_lib/visual_object.h.gcov.png | Bin 0 -> 484 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 | 168 + .../batch_visualizer.cpp.func.html | 168 + .../batch_visualizer.cpp.gcov.frameset.html | 19 + .../batch_visualizer.cpp.gcov.html | 432 + .../batch_visualizer.cpp.gcov.overview.html | 107 + .../batch_visualizer.cpp.gcov.png | Bin 0 -> 1246 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 | 164 + .../visual_object.cpp.func.html | 164 + .../visual_object.cpp.gcov.frameset.html | 19 + .../visual_object.cpp.gcov.html | 401 + .../visual_object.cpp.gcov.overview.html | 100 + .../visual_object.cpp.gcov.png | Bin 0 -> 1166 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 -> 760 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 -> 841 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 | 156 + .../src/automatic_start.cpp.func.html | 156 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 887 ++ .../automatic_start.cpp.gcov.overview.html | 221 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 2658 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 -> 533 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 -> 1823 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 -> 6650 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 -> 5820 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 -> 918 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 -> 1253 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 | 701 ++ .../tf_source.h.gcov.overview.html | 175 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2207 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 | 706 ++ .../constraint_manager.cpp.gcov.overview.html | 176 + .../src/constraint_manager.cpp.gcov.png | Bin 0 -> 2171 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 | 1301 +++ .../common/common.cpp.gcov.overview.html | 325 + .../common/common.cpp.gcov.png | Bin 0 -> 2387 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 | 8819 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2204 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 25875 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 | 1313 +++ .../estimation_manager.cpp.gcov.overview.html | 328 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4305 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 -> 799 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 | 720 ++ .../src/gain_manager.cpp.gcov.overview.html | 179 + .../src/gain_manager.cpp.gcov.png | Bin 0 -> 2109 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 -> 818 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 | 132 + .../transform_manager.cpp.func.html | 132 + .../transform_manager.cpp.gcov.frameset.html | 19 + .../transform_manager.cpp.gcov.html | 1030 ++ .../transform_manager.cpp.gcov.overview.html | 257 + .../transform_manager.cpp.gcov.png | Bin 0 -> 3294 bytes .../src/uav_manager.cpp.func-sort-c.html | 232 + .../src/uav_manager.cpp.func.html | 232 + .../src/uav_manager.cpp.gcov.frameset.html | 19 + .../src/uav_manager.cpp.gcov.html | 2732 +++++ .../src/uav_manager.cpp.gcov.overview.html | 682 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7136 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 | 242 + .../altitude/alt_generic.h.gcov.overview.html | 60 + .../altitude/alt_generic.h.gcov.png | Bin 0 -> 682 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 | 348 + .../estimators/correction.h.func.html | 348 + .../correction.h.gcov.frameset.html | 19 + .../estimators/correction.h.gcov.html | 1795 ++++ .../correction.h.gcov.overview.html | 448 + .../estimators/correction.h.gcov.png | Bin 0 -> 5088 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 | 240 + .../heading/hdg_generic.h.gcov.overview.html | 59 + .../estimators/heading/hdg_generic.h.gcov.png | Bin 0 -> 655 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 | 244 + .../lateral/lat_generic.h.gcov.overview.html | 60 + .../estimators/lateral/lat_generic.h.gcov.png | Bin 0 -> 682 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 | 222 + .../proc_tf_to_world.h.gcov.overview.html | 55 + .../processors/proc_tf_to_world.h.gcov.png | Bin 0 -> 689 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 -> 1039 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 | 200 + .../altitude/alt_generic.cpp.func.html | 200 + .../alt_generic.cpp.gcov.frameset.html | 19 + .../altitude/alt_generic.cpp.gcov.html | 797 ++ .../alt_generic.cpp.gcov.overview.html | 199 + .../altitude/alt_generic.cpp.gcov.png | Bin 0 -> 2791 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 | 204 + .../heading/hdg_generic.cpp.func.html | 204 + .../hdg_generic.cpp.gcov.frameset.html | 19 + .../heading/hdg_generic.cpp.gcov.html | 754 ++ .../hdg_generic.cpp.gcov.overview.html | 188 + .../heading/hdg_generic.cpp.gcov.png | Bin 0 -> 2195 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 | 387 + .../hdg_passthrough.cpp.gcov.overview.html | 96 + .../heading/hdg_passthrough.cpp.gcov.png | Bin 0 -> 1189 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 | 200 + .../lateral/lat_generic.cpp.func.html | 200 + .../lat_generic.cpp.gcov.frameset.html | 19 + .../lateral/lat_generic.cpp.gcov.html | 838 ++ .../lat_generic.cpp.gcov.overview.html | 209 + .../lateral/lat_generic.cpp.gcov.png | Bin 0 -> 2974 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 | 182 + .../estimators/state/index-detail-sort-l.html | 182 + .../src/estimators/state/index-detail.html | 182 + .../src/estimators/state/index-sort-f.html | 142 + .../src/estimators/state/index-sort-l.html | 142 + .../src/estimators/state/index.html | 142 + .../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 -> 1098 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/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 -> 1971 bytes mrs_uav_testing/src/index-detail-sort-f.html | 110 + mrs_uav_testing/src/index-detail-sort-l.html | 110 + mrs_uav_testing/src/index-detail.html | 110 + mrs_uav_testing/src/index-sort-f.html | 102 + mrs_uav_testing/src/index-sort-l.html | 102 + mrs_uav_testing/src/index.html | 102 + .../src/test_generic.cpp.func-sort-c.html | 132 + .../src/test_generic.cpp.func.html | 132 + .../src/test_generic.cpp.gcov.frameset.html | 19 + .../src/test_generic.cpp.gcov.html | 588 ++ .../src/test_generic.cpp.gcov.overview.html | 146 + mrs_uav_testing/src/test_generic.cpp.gcov.png | Bin 0 -> 1400 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 -> 1757 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 -> 4783 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 -> 3773 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 | 276 + .../src/mpc_tracker/mpc_tracker.cpp.func.html | 276 + .../mpc_tracker.cpp.gcov.frameset.html | 19 + .../src/mpc_tracker/mpc_tracker.cpp.gcov.html | 3966 ++++++++ .../mpc_tracker.cpp.gcov.overview.html | 991 ++ .../src/mpc_tracker/mpc_tracker.cpp.gcov.png | Bin 0 -> 12679 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 -> 3030 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 | 84 + .../eth_mav_msgs/eigen_mav_msgs.h.func.html | 84 + .../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 -> 1483 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 | 80 + .../extremum.h.func.html | 80 + .../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 -> 2760 bytes ...mization_nonlinear_impl.h.func-sort-c.html | 152 + ...al_optimization_nonlinear_impl.h.func.html | 152 + ...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 -> 3492 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 | 104 + .../polynomial.h.func.html | 104 + .../polynomial.h.gcov.frameset.html | 19 + .../polynomial.h.gcov.html | 353 + .../polynomial.h.gcov.overview.html | 88 + .../polynomial.h.gcov.png | Bin 0 -> 1546 bytes ...ial_optimization_linear.h.func-sort-c.html | 84 + ...polynomial_optimization_linear.h.func.html | 84 + ...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 -> 1756 bytes ..._optimization_nonlinear.h.func-sort-c.html | 80 + ...ynomial_optimization_nonlinear.h.func.html | 80 + ...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 | 84 + .../segment.h.func.html | 84 + .../segment.h.gcov.frameset.html | 19 + .../segment.h.gcov.html | 231 + .../segment.h.gcov.overview.html | 57 + .../segment.h.gcov.png | Bin 0 -> 899 bytes .../timing.h.func-sort-c.html | 88 + .../timing.h.func.html | 88 + .../timing.h.gcov.frameset.html | 19 + .../timing.h.gcov.html | 322 + .../timing.h.gcov.overview.html | 80 + .../timing.h.gcov.png | Bin 0 -> 1036 bytes .../trajectory.h.func-sort-c.html | 92 + .../trajectory.h.func.html | 92 + .../trajectory.h.gcov.frameset.html | 19 + .../trajectory.h.gcov.html | 261 + .../trajectory.h.gcov.overview.html | 65 + .../trajectory.h.gcov.png | Bin 0 -> 979 bytes .../vertex.h.func-sort-c.html | 80 + .../vertex.h.func.html | 80 + .../vertex.h.gcov.frameset.html | 19 + .../vertex.h.gcov.html | 256 + .../vertex.h.gcov.overview.html | 63 + .../vertex.h.gcov.png | Bin 0 -> 1058 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 -> 468 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 -> 4823 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 -> 1506 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 -> 1087 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 -> 2544 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 | 670 ++ .../vertex.cpp.gcov.overview.html | 167 + .../vertex.cpp.gcov.png | Bin 0 -> 2255 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 | 172 + .../mrs_trajectory_generation.cpp.func.html | 172 + ...ajectory_generation.cpp.gcov.frameset.html | 19 + .../mrs_trajectory_generation.cpp.gcov.html | 2416 +++++ ...ajectory_generation.cpp.gcov.overview.html | 603 ++ .../mrs_trajectory_generation.cpp.gcov.png | Bin 0 -> 7770 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 1164 files changed, 197685 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/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/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/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_testing/src/index-detail-sort-f.html create mode 100644 mrs_uav_testing/src/index-detail-sort-l.html create mode 100644 mrs_uav_testing/src/index-detail.html create mode 100644 mrs_uav_testing/src/index-sort-f.html create mode 100644 mrs_uav_testing/src/index-sort-l.html create mode 100644 mrs_uav_testing/src/index.html create mode 100644 mrs_uav_testing/src/test_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_testing/src/test_generic.cpp.func.html create mode 100644 mrs_uav_testing/src/test_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_testing/src/test_generic.cpp.gcov.html create mode 100644 mrs_uav_testing/src/test_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_testing/src/test_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 coverage59.0%59.0% \ No newline at end of file diff --git a/emerald.png b/emerald.png new file mode 100644 index 0000000000000000000000000000000000000000..38ad4f4068b935643d2486f323005fb294a9bd7e GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^Jb!lvI6;R0X`wF(yt=9xVZRt1vCRixIA4P dLn>}1Cji+@42)0J?}79&c)I$ztaD0e0sy@GAL0N2 literal 0 HcmV?d00001 diff --git a/gcov.css b/gcov.css new file mode 100644 index 0000000000..bfd0a83e10 --- /dev/null +++ b/gcov.css @@ -0,0 +1,519 @@ +/* All views: initial background and text color */ +body +{ + color: #000000; + background-color: #FFFFFF; +} + +/* All views: standard link format*/ +a:link +{ + color: #284FA8; + text-decoration: underline; +} + +/* All views: standard link - visited format */ +a:visited +{ + color: #00CB40; + text-decoration: underline; +} + +/* All views: standard link - activated format */ +a:active +{ + color: #FF0040; + text-decoration: underline; +} + +/* All views: main title format */ +td.title +{ + text-align: center; + padding-bottom: 10px; + font-family: sans-serif; + font-size: 20pt; + font-style: italic; + font-weight: bold; +} + +/* All views: header item format */ +td.headerItem +{ + text-align: right; + padding-right: 6px; + font-family: sans-serif; + font-weight: bold; + vertical-align: top; + white-space: nowrap; +} + +/* All views: header item value format */ +td.headerValue +{ + text-align: left; + color: #284FA8; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; +} + +/* All views: header item coverage table heading */ +td.headerCovTableHead +{ + text-align: center; + padding-right: 6px; + padding-left: 6px; + padding-bottom: 0px; + font-family: sans-serif; + font-size: 80%; + white-space: nowrap; +} + +/* All views: header item coverage table entry */ +td.headerCovTableEntry +{ + text-align: right; + color: #284FA8; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #DAE7FE; +} + +/* All views: header item coverage table entry for high coverage rate */ +td.headerCovTableEntryHi +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #A7FC9D; +} + +/* All views: header item coverage table entry for medium coverage rate */ +td.headerCovTableEntryMed +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #FFEA20; +} + +/* All views: header item coverage table entry for ow coverage rate */ +td.headerCovTableEntryLo +{ + text-align: right; + color: #000000; + font-family: sans-serif; + font-weight: bold; + white-space: nowrap; + padding-left: 12px; + padding-right: 4px; + background-color: #FF0000; +} + +/* All views: header legend value for legend entry */ +td.headerValueLeg +{ + text-align: left; + color: #000000; + font-family: sans-serif; + font-size: 80%; + white-space: nowrap; + padding-top: 4px; +} + +/* All views: color of horizontal ruler */ +td.ruler +{ + background-color: #6688D4; +} + +/* All views: version string format */ +td.versionInfo +{ + text-align: center; + padding-top: 2px; + font-family: sans-serif; + font-style: italic; +} + +/* Directory view/File view (all)/Test case descriptions: + table headline format */ +td.tableHead +{ + text-align: center; + color: #FFFFFF; + background-color: #6688D4; + font-family: sans-serif; + font-size: 120%; + font-weight: bold; + white-space: nowrap; + padding-left: 4px; + padding-right: 4px; +} + +span.tableHeadSort +{ + padding-right: 4px; +} + +/* Directory view/File view (all): filename entry format */ +td.coverFile +{ + text-align: left; + padding-left: 10px; + padding-right: 20px; + color: #284FA8; + background-color: #DAE7FE; + font-family: monospace; +} + +/* Directory view/File view (all): bar-graph entry format*/ +td.coverBar +{ + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; +} + +/* Directory view/File view (all): bar-graph outline color */ +td.coverBarOutline +{ + background-color: #000000; +} + +/* Directory view/File view (all): percentage entry for files with + high coverage rate */ +td.coverPerHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #A7FC9D; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + high coverage rate */ +td.coverNumHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #A7FC9D; + white-space: nowrap; + font-family: sans-serif; +} + +/* Directory view/File view (all): percentage entry for files with + medium coverage rate */ +td.coverPerMed +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FFEA20; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + medium coverage rate */ +td.coverNumMed +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FFEA20; + white-space: nowrap; + font-family: sans-serif; +} + +/* Directory view/File view (all): percentage entry for files with + low coverage rate */ +td.coverPerLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + font-weight: bold; + font-family: sans-serif; +} + +/* Directory view/File view (all): line count entry for files with + low coverage rate */ +td.coverNumLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + white-space: nowrap; + font-family: sans-serif; +} + +/* File view (all): "show/hide details" link format */ +a.detail:link +{ + color: #B8D0FF; + font-size:80%; +} + +/* File view (all): "show/hide details" link - visited format */ +a.detail:visited +{ + color: #B8D0FF; + font-size:80%; +} + +/* File view (all): "show/hide details" link - activated format */ +a.detail:active +{ + color: #FFFFFF; + font-size:80%; +} + +/* File view (detail): test name entry */ +td.testName +{ + text-align: right; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* File view (detail): test percentage entry */ +td.testPer +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* File view (detail): test lines count entry */ +td.testNum +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-family: sans-serif; +} + +/* Test case descriptions: test name format*/ +dt +{ + font-family: sans-serif; + font-weight: bold; +} + +/* Test case descriptions: description table body */ +td.testDescription +{ + padding-top: 10px; + padding-left: 30px; + padding-bottom: 10px; + padding-right: 30px; + background-color: #DAE7FE; +} + +/* Source code view: function entry */ +td.coverFn +{ + text-align: left; + padding-left: 10px; + padding-right: 20px; + color: #284FA8; + background-color: #DAE7FE; + font-family: monospace; +} + +/* Source code view: function entry zero count*/ +td.coverFnLo +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #FF0000; + font-weight: bold; + font-family: sans-serif; +} + +/* Source code view: function entry nonzero count*/ +td.coverFnHi +{ + text-align: right; + padding-left: 10px; + padding-right: 10px; + background-color: #DAE7FE; + font-weight: bold; + font-family: sans-serif; +} + +/* Source code view: source code format */ +pre.source +{ + font-family: monospace; + white-space: pre; + margin-top: 2px; +} + +/* Source code view: line number format */ +span.lineNum +{ + background-color: #EFE383; +} + +/* Source code view: format for lines which were executed */ +td.lineCov, +span.lineCov +{ + background-color: #CAD7FE; +} + +/* Source code view: format for Cov legend */ +span.coverLegendCov +{ + padding-left: 10px; + padding-right: 10px; + padding-bottom: 2px; + background-color: #CAD7FE; +} + +/* Source code view: format for lines which were not executed */ +td.lineNoCov, +span.lineNoCov +{ + background-color: #FF6230; +} + +/* Source code view: format for NoCov legend */ +span.coverLegendNoCov +{ + padding-left: 10px; + padding-right: 10px; + padding-bottom: 2px; + background-color: #FF6230; +} + +/* Source code view (function table): standard link - visited format */ +td.lineNoCov > a:visited, +td.lineCov > a:visited +{ + color: black; + text-decoration: underline; +} + +/* Source code view: format for lines which were executed only in a + previous version */ +span.lineDiffCov +{ + background-color: #B5F7AF; +} + +/* Source code view: format for branches which were executed + * and taken */ +span.branchCov +{ + background-color: #CAD7FE; +} + +/* Source code view: format for branches which were executed + * but not taken */ +span.branchNoCov +{ + background-color: #FF6230; +} + +/* Source code view: format for branches which were not executed */ +span.branchNoExec +{ + background-color: #FF6230; +} + +/* Source code view: format for the source code heading line */ +pre.sourceHeading +{ + white-space: pre; + font-family: monospace; + font-weight: bold; + margin: 0px; +} + +/* All views: header legend value for low rate */ +td.headerValueLegL +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 4px; + padding-right: 2px; + background-color: #FF0000; + font-size: 80%; +} + +/* All views: header legend value for med rate */ +td.headerValueLegM +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 2px; + padding-right: 2px; + background-color: #FFEA20; + font-size: 80%; +} + +/* All views: header legend value for hi rate */ +td.headerValueLegH +{ + font-family: sans-serif; + text-align: center; + white-space: nowrap; + padding-left: 2px; + padding-right: 4px; + background-color: #A7FC9D; + font-size: 80%; +} + +/* All views except source code view: legend format for low coverage */ +span.coverLegendCovLo +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #FF0000; +} + +/* All views except source code view: legend format for med coverage */ +span.coverLegendCovMed +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #FFEA20; +} + +/* All views except source code view: legend format for hi coverage */ +span.coverLegendCovHi +{ + padding-left: 10px; + padding-right: 10px; + padding-top: 2px; + background-color: #A7FC9D; +} diff --git a/glass.png b/glass.png new file mode 100644 index 0000000000000000000000000000000000000000..e1abc00680a3093c49fdb775ae6bdb6764c95af2 GIT binary patch literal 167 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)gaEa{HEjtmSN`?>!lvI6;R0X`wF z|Ns97GD8ntt^-nxB|(0{3=Yq3q=7g|-tI089jvk*Kn`btM`SSr1Gf+eGhVt|_XjA* zUgGKN%6^Gmn4d%Ph(nkFP>9RZ#WAE}PI3Z}&BVayv3^M*kj3EX>gTe~DWM4f=_Dpv literal 0 HcmV?d00001 diff --git a/index-sort-f.html b/index-sort-f.html new file mode 100644 index 0000000000..b851f104ba --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,672 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:138252340159.1 %
Date:2023-12-06 07:59:45Functions:2448414659.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/geometry +
14.6%14.6%
+ 		14.6 %65 / 44514.9 %14 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.9%18.9%
+ 		18.9 %88 / 46622.2 %12 / 54
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
42.0%42.0%
+ 		42.0 %417 / 99429.5 %31 / 105
mrs_uav_trackers/src/speed_tracker +
14.1%14.1%
+ 		14.1 %58 / 41136.8 %7 / 19
mrs_uav_trackers/src/joy_tracker +
44.4%44.4%
+ 		44.4 %63 / 14238.9 %7 / 18
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9340.0 %48 / 120
mrs_lib/src/batch_visualizer +
47.1%47.1%
+ 		47.1 %192 / 40846.5 %20 / 43
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.8%71.8%
+ 		71.8 %125 / 17450.0 %17 / 34
mrs_lib/include/mrs_lib/impl +
80.0%80.0%
+ 		80.0 %341 / 42655.6 %724 / 1302
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_managers/include/transform_manager +
47.1%47.1%
+ 		47.1 %186 / 39557.9 %11 / 19
mrs_lib/include/mrs_lib +
76.5%76.5%
+ 		76.5 %738 / 96559.2 %750 / 1267
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
46.6%46.6%
+ 		46.6 %291 / 62559.4 %19 / 32
mrs_uav_managers/src/control_manager +
51.2%51.2%
+ 		51.2 %1901 / 371259.3 %73 / 123
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_trackers/src/mpc_tracker +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_trackers/src/landoff_tracker +
71.6%71.6%
+ 		71.6 %424 / 59267.7 %21 / 31
mrs_uav_managers/src/control_manager/common +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
mrs_uav_state_estimators/src/estimators/state +
70.3%70.3%
+ 		70.3 %306 / 43572.7 %24 / 33
mrs_uav_state_estimators/src/estimators/altitude +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
mrs_uav_managers/src +
55.3%55.3%
+ 		55.3 %935 / 169074.0 %54 / 73
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
58.5%58.5%
+ 		58.5 %404 / 69174.7 %68 / 91
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
64.9%64.9%
+ 		64.9 %120 / 18576.9 %10 / 13
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_trajectory_generation/src +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
mrs_uav_managers/src/estimation_manager +
63.8%63.8%
+ 		63.8 %391 / 61378.7 %37 / 47
mrs_uav_managers/include/mrs_uav_managers/control_manager +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 27
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
81.7%81.7%
+ 		81.7 %89 / 10983.3 %15 / 18
mrs_uav_state_estimators/src/estimators/lateral +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
mrs_uav_testing/src +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
mrs_uav_controllers/src +
78.8%78.8%
+ 		78.8 %1712 / 217286.4 %57 / 66
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/profiler +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
mrs_uav_managers/src/transform_manager +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_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_uav_trajectory_generation/include/eth_mav_msgs +
100.0%
+ 		100.0 %28 / 28100.0 %3 / 3
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
88.9%88.9%
+ 		88.9 %346 / 389100.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 +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
mrs_uav_autostart/src +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index-sort-l.html b/index-sort-l.html new file mode 100644 index 0000000000..f1dcc3d25a --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,672 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:138252340159.1 %
Date:2023-12-06 07:59:45Functions:2448414659.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_uav_trackers/src/speed_tracker +
14.1%14.1%
+ 		14.1 %58 / 41136.8 %7 / 19
mrs_lib/src/geometry +
14.6%14.6%
+ 		14.6 %65 / 44514.9 %14 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.9%18.9%
+ 		18.9 %88 / 46622.2 %12 / 54
mrs_lib/src/profiler +
38.6%38.6%
+ 		38.6 %39 / 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.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
42.0%42.0%
+ 		42.0 %417 / 99429.5 %31 / 105
mrs_uav_trajectory_generation/src +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
mrs_uav_trackers/src/joy_tracker +
44.4%44.4%
+ 		44.4 %63 / 14238.9 %7 / 18
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
46.6%46.6%
+ 		46.6 %291 / 62559.4 %19 / 32
mrs_uav_managers/include/transform_manager +
47.1%47.1%
+ 		47.1 %186 / 39557.9 %11 / 19
mrs_lib/src/batch_visualizer +
47.1%47.1%
+ 		47.1 %192 / 40846.5 %20 / 43
mrs_uav_managers/src/control_manager +
51.2%51.2%
+ 		51.2 %1901 / 371259.3 %73 / 123
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_managers/src +
55.3%55.3%
+ 		55.3 %935 / 169074.0 %54 / 73
mrs_uav_state_estimators/src/estimators/altitude +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
mrs_uav_state_estimators/src/estimators/lateral +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
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 +
58.5%58.5%
+ 		58.5 %404 / 69174.7 %68 / 91
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_uav_managers/src/estimation_manager +
63.8%63.8%
+ 		63.8 %391 / 61378.7 %37 / 47
mrs_uav_managers/include/mrs_uav_managers/control_manager +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 27
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
64.9%64.9%
+ 		64.9 %120 / 18576.9 %10 / 13
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_trackers/src/mpc_tracker +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
mrs_uav_state_estimators/src/estimators/state +
70.3%70.3%
+ 		70.3 %306 / 43572.7 %24 / 33
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9340.0 %48 / 120
mrs_uav_trackers/src/landoff_tracker +
71.6%71.6%
+ 		71.6 %424 / 59267.7 %21 / 31
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.8%71.8%
+ 		71.8 %125 / 17450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_lib/include/mrs_lib +
76.5%76.5%
+ 		76.5 %738 / 96559.2 %750 / 1267
mrs_uav_managers/src/transform_manager +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
mrs_uav_controllers/src +
78.8%78.8%
+ 		78.8 %1712 / 217286.4 %57 / 66
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_lib/include/mrs_lib/impl +
80.0%80.0%
+ 		80.0 %341 / 42655.6 %724 / 1302
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
81.7%81.7%
+ 		81.7 %89 / 10983.3 %15 / 18
mrs_uav_testing/src +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
mrs_uav_autostart/src +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
88.9%88.9%
+ 		88.9 %346 / 389100.0 %12 / 12
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
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_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_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
mrs_uav_trajectory_generation/include/eth_mav_msgs +
100.0%
+ 		100.0 %28 / 28100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index.html b/index.html new file mode 100644 index 0000000000..a99c6f11f5 --- /dev/null +++ b/index.html @@ -0,0 +1,672 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:138252340159.1 %
Date:2023-12-06 07:59:45Functions:2448414659.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib +
76.5%76.5%
+ 		76.5 %738 / 96559.2 %750 / 1267
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9340.0 %48 / 120
mrs_lib/include/mrs_lib/impl +
80.0%80.0%
+ 		80.0 %341 / 42655.6 %724 / 1302
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 +
47.1%47.1%
+ 		47.1 %192 / 40846.5 %20 / 43
mrs_lib/src/geometry +
14.6%14.6%
+ 		14.6 %65 / 44514.9 %14 / 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 +
38.6%38.6%
+ 		38.6 %39 / 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 +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
mrs_uav_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_uav_controllers/src +
78.8%78.8%
+ 		78.8 %1712 / 217286.4 %57 / 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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 27
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
81.7%81.7%
+ 		81.7 %89 / 10983.3 %15 / 18
mrs_uav_managers/include/transform_manager +
47.1%47.1%
+ 		47.1 %186 / 39557.9 %11 / 19
mrs_uav_managers/src +
55.3%55.3%
+ 		55.3 %935 / 169074.0 %54 / 73
mrs_uav_managers/src/control_manager +
51.2%51.2%
+ 		51.2 %1901 / 371259.3 %73 / 123
mrs_uav_managers/src/control_manager/common +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
mrs_uav_managers/src/estimation_manager +
63.8%63.8%
+ 		63.8 %391 / 61378.7 %37 / 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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
58.5%58.5%
+ 		58.5 %404 / 69174.7 %68 / 91
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 %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/processors +
71.8%71.8%
+ 		71.8 %125 / 17450.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 +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
mrs_uav_state_estimators/src/estimators/heading +
18.9%18.9%
+ 		18.9 %88 / 46622.2 %12 / 54
mrs_uav_state_estimators/src/estimators/lateral +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
mrs_uav_state_estimators/src/estimators/state +
70.3%70.3%
+ 		70.3 %306 / 43572.7 %24 / 33
mrs_uav_testing/src +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
mrs_uav_trackers/src/joy_tracker +
44.4%44.4%
+ 		44.4 %63 / 14238.9 %7 / 18
mrs_uav_trackers/src/landoff_tracker +
71.6%71.6%
+ 		71.6 %424 / 59267.7 %21 / 31
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %323 / 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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
mrs_uav_trackers/src/speed_tracker +
14.1%14.1%
+ 		14.1 %58 / 41136.8 %7 / 19
mrs_uav_trajectory_generation/include/eth_mav_msgs +
100.0%
+ 		100.0 %28 / 28100.0 %3 / 3
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
64.9%64.9%
+ 		64.9 %120 / 18576.9 %10 / 13
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
46.6%46.6%
+ 		46.6 %291 / 62559.4 %19 / 32
mrs_uav_trajectory_generation/src +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
42.0%42.0%
+ 		42.0 %417 / 99429.5 %31 / 105
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
88.9%88.9%
+ 		88.9 %346 / 389100.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..2e4afe9333 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + 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:232979.3 %
Date:2023-12-06 07:59:45Functions:91275.0 %
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
mrs_lib::AttitudeConverter::EulerFormatException::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_Li0EEEIdEEv30717
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)95107
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)235192
+
+
+ + + +
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..d554f79e68 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func.html @@ -0,0 +1,128 @@ + + + + + + + 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:232979.3 %
Date:2023-12-06 07:59:45Functions:91275.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)95107
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>)235192
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
mrs_lib::AttitudeConverter::EulerFormatException::what() const0
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv30717
_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..a45f079276 --- /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:232979.3 %
Date:2023-12-06 07:59:45Functions:91275.0 %
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       95107 :   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           0 :     const char* what() const throw() {
+     185           0 :       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     4011698 :   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      235192 :   AttitudeConverter(const Eigen::AngleAxis<T> angle_axis) {
+     254      234561 :     double       angle = angle_axis.angle();
+     255      234588 :     tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
+     256             : 
+     257      234865 :     tf2_quaternion_.setRotation(axis, angle);
+     258      234723 :   }
+     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       30717 :   operator Eigen::Quaternion<T>() const {
+     334             : 
+     335       30717 :     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..708a493ebf15f10db204e2e60e12329b8d395a7e GIT binary patch literal 1660 zcmV-?27~#DP)R0{{R3v^8aD0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpRn< zn<Azo}&sJLrpMfT}Pk+x>i}3hiljFHK z=1Nx63Ge-A7s_wY#}A;7K_$0eSJB5@;!Ad26IP|lfkJ+RKBhZ9E4{puJ|+}x(auDu z?H?sLe)+F(^ZMR@@_I9l>Kx;4hh8wGCYr@qx`f!S-&ue#U?kaB6m>R9jXC>0F*Gmv zD*IONA90PfVxLS6+4!nwu!Ld0RkLv=feXAR39S|zysf41%n*9@N_kZ z0nd1UO$<-I#!3b_^57YmmqdHo<|TtiWL_+IhUdx1@EACb*78CE4@_bhji)^$-%Q+{ zNuUg>$wxKo%?xoHXPSEKVf8W-QZE|vH62!(l%U}139qC)9-R^UkAz0S(hA;flFD%}&FKWlTp zTlI*9*sfdfv{^AS=rtKFNA}6bn?}T5Su@q&;*{M zu{0DpZou!0}f;Q=#?<=C6FFLhk`p zp7zw0`xGAoZtpz_n1d@;iU~*Zp}2HlDV=>YrNrQMfm7$Q@noICIr}(qQNA7_2XK(p zr&p|CkRwAMUGn)h;$IVDy9R(y2q_%%_UF*!vH;JsDTyakz->A5X~Nrm*Hn1ss|50X zxSkjIgJ;a~51t`MUT)^9{~=JF{CIEjQ9GTC6E~GrgvY=6U*W-XeD~t4BuNHfWNey2 zdV*x<05e?5#Q4cdQJvB-65qlE+}R!Gg$)_-sRY*nTnzUqR#+xs^4O4z%Ly$R@JKM9 z5Cbd110it=1;_6W9GNy_pd3|MfH0VnuXDxUaik6=21zg`TQ9p=gAv3v$WfwZ;V1lx z6GUE0#TDb|rj@G)ZMNg8f5IQ#&>kh0eqeD(T5M_+Sw$Rgc}Bxtlj(*NS9C!ONE5$u z@4k+7+!Kse)q8q9w?-|l1J5=s;VGx>wf&&fy<>{J{80000 + + + + + + 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:2023-12-06 07:59:45Functions: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&)39
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)39
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)39
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&)39
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_defaults(mrs_uav_state_estimators::AltitudeEstimatorConfig&)74
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)74
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)74
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&)74
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)78
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&)117
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&)117
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<double>(boost::any&, double*&)117
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)117
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)148
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&)154
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&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_defaults(mrs_uav_state_estimators::CorrectionConfig&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)154
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<double>(boost::any&, double*&)154
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)154
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&)154
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&)222
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&)222
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<double>(boost::any&, double*&)222
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)222
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)308
+
+
+ + + +
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..0ba539478a --- /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:2023-12-06 07:59:45Functions: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&)154
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&)154
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&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)308
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)154
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*&)154
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)154
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)154
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&)154
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&)117
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&)117
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&)39
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)78
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)39
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)39
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*&)117
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)117
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&)39
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&)222
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&)222
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&)74
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)148
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)74
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)74
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*&)222
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)222
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&)74
+
+
+ + + +
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..b0e2112091 --- /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:2023-12-06 07:59:45Functions: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         267 :   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         267 :         m_server(m_server_mtx, nh),
+      54             :         m_usr_cbf(user_callback),
+      55         267 :         m_pl(nh, print_values, node_name)
+      56             :   {
+      57             :     // initialize the dynamic reconfigure callback
+      58         267 :     m_server.setCallback(boost::bind(&DynamicReconfigureMgr<ConfigType>::dynamic_reconfigure_callback, this, _1, _2));
+      59         267 :   };
+      60             : 
+      61             :   /* Constructor overloads //{ */
+      62             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(nh, node_name)
+      63         154 :   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         534 :   void update_config(const ConfigType& cfg)
+      72             :   {
+      73         534 :     m_server.updateConfig(cfg);
+      74         534 :   }
+      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         267 :   void dynamic_reconfigure_callback(ConfigType& new_config, uint32_t level)
+     107             :   {
+     108         267 :     if (m_print_values)
+     109             :     {
+     110         267 :       if (m_node_name.empty())
+     111           0 :         ROS_INFO("Dynamic reconfigure request received");
+     112             :       else
+     113         267 :         ROS_INFO("[%s]: Dynamic reconfigure request received", m_node_name.c_str());
+     114             :     }
+     115             : 
+     116         267 :     if (m_not_initialized)
+     117             :     {
+     118         267 :       load_defaults(new_config);
+     119         267 :       update_config(new_config);
+     120             :     }
+     121         267 :     if (m_print_values)
+     122             :     {
+     123         267 :       print_changed_params(new_config);
+     124             :     }
+     125         267 :     m_not_initialized = false;
+     126         267 :     config = new_config;
+     127         267 :     if (m_usr_cbf)
+     128         113 :       m_usr_cbf(new_config, level);
+     129         267 :   }
+     130             : 
+     131             :   template <typename T>
+     132         493 :   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         986 :     boost::shared_ptr<const param_descr_t> cast_descr = boost::dynamic_pointer_cast<const param_descr_t>(descr);
+     136         493 :     m_pl.loadParam(name, config.*(cast_descr->field));
+     137         493 :   }
+     138             :   
+     139         267 :   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         534 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     143         760 :     for (auto& descr : descrs)
+     144             :     {
+     145         986 :       std::string name = descr->name;
+     146         493 :       size_t pos = name.find("__");
+     147         493 :       while (pos != name.npos)
+     148             :       {
+     149           0 :         name.replace(pos, 2, "/");
+     150           0 :         pos = name.find("__");
+     151             :       }
+     152             : 
+     153         493 :       if (descr->type == "bool")
+     154           0 :         load_param<bool>(name, descr, new_config);
+     155         493 :       else if (descr->type == "int")
+     156           0 :         load_param<int>(name, descr, new_config);
+     157         493 :       else if (descr->type == "double")
+     158         493 :         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         267 :   }
+     168             : 
+     169             :   // method for printing names and values of new received parameters (prints only the changed ones) //{
+     170         267 :   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         534 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     174         760 :     for (auto& descr : descrs)
+     175             :     {
+     176           0 :       boost::any val, old_val;
+     177         493 :       descr->getValue(new_config, val);
+     178         493 :       descr->getValue(config, old_val);
+     179         493 :       std::string name = descr->name;
+     180         493 :       const size_t pos = name.find("__");
+     181         493 :       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         493 :       if (try_cast(val, intval))
+     199             :       {
+     200           0 :         if (m_not_initialized || !try_compare(old_val, intval))
+     201           0 :           print_value(name, *intval);
+     202         493 :       } else if (try_cast(val, doubleval))
+     203             :       {
+     204         493 :         if (m_not_initialized || !try_compare(old_val, doubleval))
+     205         493 :           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         267 :   }
+     220             :   //}
+     221             :   
+     222             :   // helper method for parameter printing
+     223             :   template <typename T>
+     224         493 :   inline void print_value(const std::string& name, const T& val)
+     225             :   {
+     226         493 :     if (m_node_name.empty())
+     227           0 :       std::cout << "\t" << name << ":\t" << val << std::endl;
+     228             :     else
+     229         493 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << val);
+     230         493 :   }
+     231             :   // helper methods for automatic parameter value parsing
+     232             :   template <typename T>
+     233         986 :   inline bool try_cast(boost::any& val, T*& out)
+     234             :   {
+     235         986 :     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..39f9c2ac58 --- /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:2023-12-06 07:59:45Functions:4511738.5 %
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>::interpUnwrapped(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>::interpUnwrapped(double, double, double)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>::interp(double, double, 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::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>::dist(double, double)10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::value() const10002
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10004
mrs_lib::geometry::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::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::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)39360
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)39360
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)39360
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::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)98744
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)171201
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>)211403
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>)392610
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)402778
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)442829
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)815216
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)837160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)892040
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)903133
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>)952499
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)2004995
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)2116104
+
+
+ + + +
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..38bb012312 --- /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:2023-12-06 07:59:45Functions:4511738.5 %
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)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)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>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)392610
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)171201
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>)211403
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)837160
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)0
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&)402778
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)442829
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)815216
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)39360
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)39360
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>)952499
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)903133
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)2116104
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)39360
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)892040
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&)98744
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)2004995
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..0403a3c0aa --- /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:2023-12-06 07:59:45Functions:4511738.5 %
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     2860224 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67      541575 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73      402778 :       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     3013269 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117     3013269 :         if (val >= minimum)
+     118             :         {
+     119     2727488 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120     2553519 :             return val;
+     121      173962 :           else if (val < supremum + range)
+     122       11212 :             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      285781 :           if (val >= minimum - range)
+     126      121969 :             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      326567 :         const flt rem = std::fmod(val - minimum, range);
+     131      326567 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326561 :         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      922043 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159      922043 :         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     1094334 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195     1094334 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198     1365113 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200     1365113 :         const flt d = minuend.val - subtrahend.val;
+     201     1365113 :         if (d < -half_range)
+     202       16866 :           return d + range;
+     203     1348247 :         if (d >= half_range)
+     204       15474 :           return d - range;
+     205     1332769 :         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       20002 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20002 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224      221413 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226      221413 :         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       39360 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244       39360 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247       39360 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249       39360 :         const flt dang = diff(to, from);
+     250       39360 :         const flt intp = from.val + coeff * dang;
+     251       39360 :         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       39360 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266       39360 :         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:2023-12-06 07:59:45Functions:4812040.0 %
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 / 8738.5 %45 / 117
<unnamed>69.0 %60 / 8738.5 %45 / 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..ffcd481ca2 --- /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:2023-12-06 07:59:45Functions:4812040.0 %
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 / 8738.5 %45 / 117
<unnamed>69.0 %60 / 8738.5 %45 / 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..ad7f25e319 --- /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:2023-12-06 07:59:45Functions:4812040.0 %
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 / 8738.5 %45 / 117
<unnamed>69.0 %60 / 8738.5 %45 / 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..b4b1596629 --- /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:2023-12-06 07:59:45Functions:4812040.0 %
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 / 8738.5 %45 / 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..7313550dce --- /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:2023-12-06 07:59:45Functions:4812040.0 %
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 / 8738.5 %45 / 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..af67ba971d --- /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:2023-12-06 07:59:45Functions:4812040.0 %
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 / 8738.5 %45 / 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..f9428b156c --- /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:2023-12-06 07:59:45Functions: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&)31424
+
+
+ + + +
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..913b06e114 --- /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:2023-12-06 07:59:45Functions: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&)31424
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..6657e03bf7 --- /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:2023-12-06 07:59:45Functions: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       31430 :     double headingFromRot(const T& rot)
+      58             :     {
+      59       31430 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60       62860 :       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:2023-12-06 07:59:45Functions: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&)492
mrs_lib::UTM(double, double, double*, double*)2244
mrs_lib::LLtoUTM(double, double, double&, double&, char*)60781
mrs_lib::UTMLetterDesignator(double)60797
+
+
+ + + +
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..3f79360282 --- /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:2023-12-06 07:59:45Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)60797
mrs_lib::UTM(double, double, double*, double*)2244
mrs_lib::LLtoUTM(double, double, double&, double&, char*)60781
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)492
+
+
+ + + +
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..6bb86f042f --- /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:2023-12-06 07:59:45Functions: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        2244 :   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        2244 :     int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
+      63             : 
+      64             :     // convert degrees into radians
+      65        2244 :     double rlat  = lat * RADIANS_PER_DEGREE;
+      66        2244 :     double rlon  = lon * RADIANS_PER_DEGREE;
+      67        2244 :     double rlon0 = cm * RADIANS_PER_DEGREE;
+      68             : 
+      69             :     // compute trigonometric functions
+      70        2244 :     double slat = sin(rlat);
+      71        2244 :     double clat = cos(rlat);
+      72        2244 :     double tlat = tan(rlat);
+      73             : 
+      74             :     // decide the false northing at origin
+      75        2244 :     double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
+      76             : 
+      77        2244 :     double T = tlat * tlat;
+      78        2244 :     double C = UTM_EP2 * clat * clat;
+      79        2244 :     double A = (rlon - rlon0) * clat;
+      80        2244 :     double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
+      81        2244 :     double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
+      82             : 
+      83             :     // compute the easting-northing coordinates
+      84        2244 :     *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        2244 :     *y = fn +
+      86        2244 :          UTM_K0 *
+      87        2244 :              (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        2244 :     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       60797 :   static inline char UTMLetterDesignator(double Lat) {
+     102             :     char LetterDesignator;
+     103             : 
+     104       60797 :     if ((84 >= Lat) && (Lat >= 72))
+     105           0 :       LetterDesignator = 'X';
+     106       60797 :     else if ((72 > Lat) && (Lat >= 64))
+     107           0 :       LetterDesignator = 'W';
+     108       60797 :     else if ((64 > Lat) && (Lat >= 56))
+     109           0 :       LetterDesignator = 'V';
+     110       60797 :     else if ((56 > Lat) && (Lat >= 48))
+     111           0 :       LetterDesignator = 'U';
+     112       60797 :     else if ((48 > Lat) && (Lat >= 40))
+     113       60462 :       LetterDesignator = 'T';
+     114         335 :     else if ((40 > Lat) && (Lat >= 32))
+     115           0 :       LetterDesignator = 'S';
+     116         335 :     else if ((32 > Lat) && (Lat >= 24))
+     117           0 :       LetterDesignator = 'R';
+     118         335 :     else if ((24 > Lat) && (Lat >= 16))
+     119           0 :       LetterDesignator = 'Q';
+     120         335 :     else if ((16 > Lat) && (Lat >= 8))
+     121           0 :       LetterDesignator = 'P';
+     122         335 :     else if ((8 > Lat) && (Lat >= 0))
+     123           2 :       LetterDesignator = 'N';
+     124         333 :     else if ((0 > Lat) && (Lat >= -8))
+     125           0 :       LetterDesignator = 'M';
+     126         333 :     else if ((-8 > Lat) && (Lat >= -16))
+     127           0 :       LetterDesignator = 'L';
+     128         333 :     else if ((-16 > Lat) && (Lat >= -24))
+     129           0 :       LetterDesignator = 'K';
+     130         333 :     else if ((-24 > Lat) && (Lat >= -32))
+     131           0 :       LetterDesignator = 'J';
+     132         333 :     else if ((-32 > Lat) && (Lat >= -40))
+     133           0 :       LetterDesignator = 'H';
+     134         333 :     else if ((-40 > Lat) && (Lat >= -48))
+     135           0 :       LetterDesignator = 'G';
+     136         333 :     else if ((-48 > Lat) && (Lat >= -56))
+     137           0 :       LetterDesignator = 'F';
+     138         333 :     else if ((-56 > Lat) && (Lat >= -64))
+     139           0 :       LetterDesignator = 'E';
+     140         333 :     else if ((-64 > Lat) && (Lat >= -72))
+     141           0 :       LetterDesignator = 'D';
+     142         333 :     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         333 :       LetterDesignator = 'Z';
+     147       60797 :     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       60781 :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
+     160       60781 :     double a          = WGS84_A;
+     161       60781 :     double eccSquared = UTM_E2;
+     162       60781 :     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       60781 :     double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
+     170             : 
+     171       60781 :     double LatRad  = Lat * RADIANS_PER_DEGREE;
+     172       60781 :     double LongRad = LongTemp * RADIANS_PER_DEGREE;
+     173             :     double LongOriginRad;
+     174             :     int    ZoneNumber;
+     175             : 
+     176       60781 :     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       60781 :     if (ZoneNumber > 99)
+     180           0 :       ZoneNumber = 99;
+     181       60781 :     if (ZoneNumber < -9)
+     182           0 :       ZoneNumber = -9;
+     183             : 
+     184       60781 :     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+     185           0 :       ZoneNumber = 32;
+     186             : 
+     187             :     // Special zones for Svalbard
+     188       60781 :     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       60781 :     LongOrigin    = (ZoneNumber - 1) * 6 - 180 + 3;
+     200       60781 :     LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
+     201             : 
+     202             :     // compute the UTM Zone from the latitude and longitude
+     203       60781 :     snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+     204             : 
+     205       60614 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     206             : 
+     207       60614 :     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     208       60614 :     T = tan(LatRad) * tan(LatRad);
+     209       60614 :     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     210       60614 :     A = cos(LatRad) * (LongRad - LongOriginRad);
+     211             : 
+     212       60614 :     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
+     213       60614 :              (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
+     214       60614 :              (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
+     215       60614 :              (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+     216             : 
+     217       60614 :     UTMEasting =
+     218       60614 :         (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       60614 :     UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
+     221       60614 :                                          (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+     222       60614 :                                           (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     223       60614 :     if (Lat < 0)
+     224           0 :       UTMNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+     225       60614 :   }
+     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         492 :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
+     246         492 :     double                  k0         = UTM_K0;
+     247         492 :     double                  a          = WGS84_A;
+     248         492 :     double                  eccSquared = UTM_E2;
+     249             :     double                  eccPrimeSquared;
+     250         492 :     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         492 :     x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
+     261         492 :     y = UTMNorthing;
+     262             : 
+     263         492 :     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     264         492 :     if ((*ZoneLetter - 'N') >= 0)
+     265         492 :       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         492 :     LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
+     272             : 
+     273         492 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     274             : 
+     275         492 :     M  = y / k0;
+     276         492 :     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+     277             : 
+     278         492 :     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         492 :               (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     280         492 :     phi1 = phi1Rad * DEGREES_PER_RADIAN;
+     281             : 
+     282         492 :     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     283         492 :     T1 = tan(phi1Rad) * tan(phi1Rad);
+     284         492 :     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     285         492 :     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     286         492 :     D  = x / (N1 * k0);
+     287             : 
+     288         492 :     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         492 :                                                 (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     290         492 :     Lat = Lat * DEGREES_PER_RADIAN;
+     291             : 
+     292         492 :     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         492 :            cos(phi1Rad);
+     294         492 :     Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+     295         492 :   }
+     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:34142680.0 %
Date:2023-12-06 07:59:45Functions:724130255.6 %
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 / 12043.9 %433 / 986
<unnamed>81.7 %98 / 12043.9 %433 / 986
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 / 6481.2 %39 / 48
<unnamed>85.9 %55 / 6481.2 %39 / 48
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
<unnamed>64.3 %9 / 1487.5 %14 / 16
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 5695.3 %41 / 43
<unnamed>92.9 %52 / 5695.3 %41 / 43
publisher_handler.hpp +
69.2%69.2%
+ 		69.2 %36 / 5295.9 %186 / 194
<unnamed>69.2 %36 / 5295.9 %186 / 194
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
+
+
+ + + + +
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..faff42581a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html @@ -0,0 +1,218 @@ + + + + + + + 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:34142680.0 %
Date:2023-12-06 07:59:45Functions:724130255.6 %
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 / 1487.5 %14 / 16
<unnamed>64.3 %9 / 1487.5 %14 / 16
publisher_handler.hpp +
69.2%69.2%
+ 		69.2 %36 / 5295.9 %186 / 194
<unnamed>69.2 %36 / 5295.9 %186 / 194
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12043.9 %433 / 986
<unnamed>81.7 %98 / 12043.9 %433 / 986
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6481.2 %39 / 48
<unnamed>85.9 %55 / 6481.2 %39 / 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 / 5695.3 %41 / 43
<unnamed>92.9 %52 / 5695.3 %41 / 43
+
+
+ + + + +
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..186cc77144 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail.html @@ -0,0 +1,218 @@ + + + + + + + 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:34142680.0 %
Date:2023-12-06 07:59:45Functions:724130255.6 %
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 / 1487.5 %14 / 16
<unnamed>64.3 %9 / 1487.5 %14 / 16
publisher_handler.hpp +
69.2%69.2%
+ 		69.2 %36 / 5295.9 %186 / 194
<unnamed>69.2 %36 / 5295.9 %186 / 194
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 5695.3 %41 / 43
<unnamed>92.9 %52 / 5695.3 %41 / 43
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12043.9 %433 / 986
<unnamed>81.7 %98 / 12043.9 %433 / 986
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 / 6481.2 %39 / 48
<unnamed>85.9 %55 / 6481.2 %39 / 48
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
+
+
+ + + + +
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..0640fbc441 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-f.html @@ -0,0 +1,162 @@ + + + + + + + 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:34142680.0 %
Date:2023-12-06 07:59:45Functions:724130255.6 %
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 / 12043.9 %433 / 986
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6481.2 %39 / 48
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 5695.3 %41 / 43
publisher_handler.hpp +
69.2%69.2%
+ 		69.2 %36 / 5295.9 %186 / 194
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
+
+
+ + + + +
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..3ac54167fb --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-l.html @@ -0,0 +1,162 @@ + + + + + + + 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:34142680.0 %
Date:2023-12-06 07:59:45Functions:724130255.6 %
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 / 1487.5 %14 / 16
publisher_handler.hpp +
69.2%69.2%
+ 		69.2 %36 / 5295.9 %186 / 194
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12043.9 %433 / 986
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6481.2 %39 / 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 / 5695.3 %41 / 43
+
+
+ + + + +
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..2a684555ac --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index.html @@ -0,0 +1,162 @@ + + + + + + + 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:34142680.0 %
Date:2023-12-06 07:59:45Functions:724130255.6 %
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 / 1487.5 %14 / 16
publisher_handler.hpp +
69.2%69.2%
+ 		69.2 %36 / 5295.9 %186 / 194
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 5695.3 %41 / 43
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12043.9 %433 / 986
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6481.2 %39 / 48
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
+
+
+ + + + +
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..b7a4978328 --- /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:2023-12-06 07:59:45Functions:141687.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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> >&) const0
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> >&) const0
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> >&) const1
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> >&) const1
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> >&) const755
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> >&) const755
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> > > >&) const1454
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> > > >&) const1454
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const2311
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const2311
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> >&) const6844
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> >&) const6844
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const8294
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const8294
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const16918
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const16918
+
+
+ + + +
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..d604244c65 --- /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:2023-12-06 07:59:45Functions:141687.5 %
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> >&) const6844
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> > > >&) const1454
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> >&) const755
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> >&) const0
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> >&) const1
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const8294
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const16918
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const2311
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> >&) const6844
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> > > >&) const1454
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> >&) const755
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> >&) const0
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> >&) const1
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const8294
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const16918
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const2311
+
+
+ + + +
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..5b6374dd60 --- /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:2023-12-06 07:59:45Functions:141687.5 %
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       36577 :   bool ParamProvider::getParam(const std::string& param_name, T& value_out) const
+      10             :   {
+      11             :     try
+      12             :     {
+      13       36577 :       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       36577 :   bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const
+      24             :   {
+      25             :     {
+      26       36577 :       const auto found_node = findYamlNode(param_name);
+      27       36577 :       if (found_node.has_value())
+      28             :       {
+      29             :         try
+      30             :         {
+      31             :           // try catch is the only type-generic option...
+      32       31468 :           value_out = found_node.value().as<T>();
+      33       31468 :           return true;
+      34             :         }
+      35           0 :         catch (const YAML::BadConversion& e)
+      36             :         {}
+      37             :       }
+      38             : 
+      39             :     }
+      40             : 
+      41        5109 :     if (m_use_rosparam)
+      42        5109 :       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:365269.2 %
Date:2023-12-06 07:59:45Functions:18619495.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<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_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)1
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<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<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&)9
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)9
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&)9
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&)39
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)39
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&)39
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&)42
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<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&)42
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)42
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&)42
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&)42
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&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)42
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&)42
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&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)42
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&)42
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&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)42
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&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)42
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&)42
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&)42
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)42
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&)43
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&)43
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)80
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)80
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)84
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&)84
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)84
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&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > 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::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&)113
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)113
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&)113
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&)126
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)126
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&)126
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&)152
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)152
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&)152
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&)168
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)168
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&)168
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)170
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&)183
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&)183
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&)245
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)245
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&)245
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&)252
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)252
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&)252
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&)308
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)308
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&)308
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)468
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)468
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)468
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)468
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)537
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)537
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)623
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)623
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1149
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1149
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)1567
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)1567
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)3007
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)3007
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)3308
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)3308
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)3835
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)3835
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)4809
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)4809
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)4901
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)4901
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)6444
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)6444
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)6444
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)6444
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)9705
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)9705
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)16480
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24482
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24482
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)29340
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)29340
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)31960
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)31960
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)40713
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)40713
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)44104
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)44104
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)48643
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)48643
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)68769
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)68769
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)98583
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)98585
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)103032
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)103035
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)137715
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)137836
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)186356
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)186492
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)196818
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)196827
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)771271
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)771323
+
+
+ + + +
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..a000bd19ab --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,856 @@ + + + + + + + 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:365269.2 %
Date:2023-12-06 07:59:45Functions:18619495.9 %
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&)44104
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&)168
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)168
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)80
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&)42
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)48643
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&)39
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)39
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)9705
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&)252
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)252
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<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&)42
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)3007
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)29340
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&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)103035
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&)245
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)245
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)31960
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&)42
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)186492
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&)152
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)152
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)537
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&)42
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)3835
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&)42
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)771271
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&)308
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)308
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)137836
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&)113
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)113
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1149
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&)43
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)85
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&)24482
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)40713
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)4809
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&)42
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)6444
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&)42
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)623
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&)42
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)468
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&)42
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)4901
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
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&)42
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)468
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&)42
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)42
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&)42
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)98585
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&)84
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)84
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)196827
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)16480
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&)183
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)170
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)1567
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&)9
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)9
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)3308
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&)42
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)42
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&)6444
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&)42
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)42
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)68769
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&)126
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)126
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)44104
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&)168
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)80
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&)42
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)48643
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&)39
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)9705
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&)252
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)3007
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)29340
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)103032
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&)245
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)31960
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)186356
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&)152
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)537
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)3835
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)771323
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&)308
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)137715
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&)113
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1149
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&)43
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&)24482
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)40713
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)4809
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)6444
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)623
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)468
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)4901
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)468
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)1
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&)42
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)98583
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&)84
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)196818
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&)183
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)1567
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&)9
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)3308
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&)42
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&)6444
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&)42
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)68769
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&)126
+
+
+ + + +
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..8f146274a9 --- /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:365269.2 %
Date:2023-12-06 07:59:45Functions:18619495.9 %
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        2774 : PublisherHandler_impl<TopicType>::PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+      23        2774 :                                                         const double& rate) {
+      24             : 
+      25             :   {
+      26        2774 :     std::scoped_lock lock(mutex_publisher_);
+      27             : 
+      28        2774 :     publisher_ = nh.advertise<TopicType>(address, buffer_size, latch);
+      29             : 
+      30        2774 :     if (rate > 0.0) {
+      31             : 
+      32         295 :       throttle_ = true;
+      33             : 
+      34         295 :       throttle_min_dt_ = 1.0 / rate;
+      35             : 
+      36             :     } else {
+      37             : 
+      38        2479 :       throttle_ = false;
+      39             : 
+      40        2479 :       throttle_min_dt_ = 0;
+      41             :     }
+      42             : 
+      43        2774 :     last_time_published_ = ros::Time(0);
+      44             :   }
+      45             : 
+      46        2774 :   publisher_initialized_ = true;
+      47        2774 : }
+      48             : 
+      49             : //}
+      50             : 
+      51             : /* publish(TopicType& msg) //{ */
+      52             : 
+      53             : template <class TopicType>
+      54     1839885 : void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
+      55             : 
+      56     1839885 :   if (!publisher_initialized_) {
+      57           0 :     return;
+      58             :   }
+      59             : 
+      60             :   {
+      61     1839582 :     std::scoped_lock lock(mutex_publisher_);
+      62             : 
+      63     1839611 :     if (throttle_) {
+      64             : 
+      65       82441 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      66       48161 :         return;
+      67             :       }
+      68             : 
+      69       34281 :       last_time_published_ = ros::Time::now();
+      70             :     }
+      71             : 
+      72             :     try {
+      73     1791451 :       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           0 : void PublisherHandler_impl<TopicType>::publish(const boost::shared_ptr<TopicType const>& msg) {
+     119             : 
+     120           0 :   if (!publisher_initialized_) {
+     121             :     return;
+     122             :   }
+     123             : 
+     124             :   {
+     125           0 :     std::scoped_lock lock(mutex_publisher_);
+     126             : 
+     127           0 :     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           0 :       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        3137 : PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
+     169             : 
+     170        3095 :   if (this == &other) {
+     171           0 :     return *this;
+     172             :   }
+     173             : 
+     174        3137 :   if (other.impl_) {
+     175        3179 :     this->impl_ = other.impl_;
+     176             :   }
+     177             : 
+     178        3095 :   return *this;
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* copy constructor //{ */
+     184             : 
+     185             : template <class TopicType>
+     186       16480 : PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
+     187             : 
+     188       16480 :   if (other.impl_) {
+     189       16480 :     this->impl_ = other.impl_;
+     190             :   }
+     191       16480 : }
+     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        2774 : PublisherHandler<TopicType>::PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+     199        2774 :                                               const double& rate) {
+     200             : 
+     201        2774 :   impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(nh, address, buffer_size, latch, rate);
+     202        2774 : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* publish(const TopicType& msg) //{ */
+     207             : 
+     208             : template <class TopicType>
+     209     1840104 : void PublisherHandler<TopicType>::publish(const TopicType& msg) {
+     210             : 
+     211     1840104 :   impl_->publish(msg);
+     212     1840672 : }
+     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           0 : void PublisherHandler<TopicType>::publish(const boost::shared_ptr<TopicType const>& msg) {
+     230             : 
+     231           0 :   impl_->publish(msg);
+     232             : }
+     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..f0839d3892eb8a0eba65adaf8ce7905cc3eba886 GIT binary patch literal 842 zcmV-Q1GW5#P)=>m zU>D|b0z~ScVXVkBsmcB+HC7d4!8eUD;62%-fC!s*h*D-NRht<)qO^dTf`%fDXq{As z)~<+dwCWx2POZAaq*knlE?PA?akpG6o5n0tOi!ulT}03_f$%Hq zBkyv`km_SzBa&LA(^L}a8B;{Lt2oc^_j{R}GR%OIRmA1i(k$!5&=DibmC7o9c&{wt z)Tp)|5!yC5-(#s%6o|Z_NSTHP3U}gTr8?t2nham`Z!3SbqS*^hUiM4o-}-I;T8mc1 z_5z;aPwF4DHj>#U@)#oChwt4g($5d9#=+FRgt+V$s%P639k9_2I(@$` z`98{{lnCB_$ud?ShxArj(}R6n?|q1Rvhlc2I3B%Ted=BJo?9Q8I9?(np6ij`&Zb<_ z&j+l9GiPD?%~$2n1pCJu>*IE=$BQa12BpXIvr7yeF`|4~(kO247mJ8cZWN(51?PKC z8l>QD$u!y^;R4pj#K%&Q$v*z~kDU-%QWn2?zjhWyMW~I=t-aZ2KYYr^$7N4Jiccy6 z_t8g}KdI=F+8zkiy8NJ;q7qFcrm1W~36C z_rNn!ST5OxYc3wX^M4>7%kV6%BRMc5{eJd5YaYIxV9cL9+{KMi&a(5d_T^2kHExfd z_)#H|$MJJ|o7r}S^hqJ~xAgNz6Cs4fLr6Ii! + + + + + + 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:2023-12-06 07:59:45Functions:414395.3 %
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&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::PathSrv>::call(mrs_msgs::PathSrv&)1
mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::PathSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&, int const&, double const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::asyncRun()2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)6
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)6
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)30
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)30
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)42
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)74
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec4> const&)74
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)74
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)93
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)93
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)93
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)135
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)136
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)163
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)163
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)325
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)364
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)365
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)511
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)513
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)555
+
+
+ + + +
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..0a5340cdef --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,252 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:414395.3 %
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&)0
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)6
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)30
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)74
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec4> const&)74
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)163
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)93
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)93
mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)135
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)325
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)364
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&)513
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)511
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)6
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)30
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)74
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)163
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)93
mrs_lib::ServiceClientHandler_impl<mrs_msgs::PathSrv>::call(mrs_msgs::PathSrv&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::PathSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)136
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)365
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&)555
+
+
+ + + +
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..787ac24284 --- /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:2023-12-06 07:59:45Functions:414395.3 %
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        1177 : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) {
+      23             : 
+      24             :   {
+      25        1177 :     std::scoped_lock lock(mutex_service_client_);
+      26             : 
+      27        1177 :     service_client_ = nh.serviceClient<ServiceType>(address);
+      28             :   }
+      29             : 
+      30        1177 :   _address_       = address;
+      31        1177 :   async_attempts_ = 1;
+      32             : 
+      33             :   /* thread_oneshot_ = std::make_shared<std::thread>(std::thread(&ServiceClientHandler_impl::threadOneshot, this, true, false)); */
+      34             : 
+      35        1177 :   service_initialized_ = true;
+      36        1177 : }
+      37             : 
+      38             : //}
+      39             : 
+      40             : /* call(ServiceType& srv) //{ */
+      41             : 
+      42             : template <class ServiceType>
+      43         743 : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv) {
+      44             : 
+      45         743 :   if (!service_initialized_) {
+      46           0 :     return false;
+      47             :   }
+      48             : 
+      49         743 :   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        1135 : ServiceClientHandler<ServiceType>& ServiceClientHandler<ServiceType>::operator=(const ServiceClientHandler<ServiceType>& other) {
+     205             : 
+     206        1135 :   if (this == &other) {
+     207           0 :     return *this;
+     208             :   }
+     209             : 
+     210        1135 :   if (other.impl_) {
+     211        1141 :     this->impl_ = other.impl_;
+     212             :   }
+     213             : 
+     214        1129 :   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        1135 : ServiceClientHandler<ServiceType>::ServiceClientHandler(ros::NodeHandle& nh, const std::string& address) {
+     235             : 
+     236        1135 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     237        1135 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : /* initialize(ros::NodeHandle& nh, const std::string& address) //{ */
+     242             : 
+     243             : template <class ServiceType>
+     244          42 : void ServiceClientHandler<ServiceType>::initialize(ros::NodeHandle& nh, const std::string& address) {
+     245             : 
+     246          42 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     247          42 : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* call(ServiceType& srv) //{ */
+     252             : 
+     253             : template <class ServiceType>
+     254         743 : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv) {
+     255             : 
+     256         743 :   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..c003eababa --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html @@ -0,0 +1,4024 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:43398643.9 %
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::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> > >::ImplThreadsafe::getMsg()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::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()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::process_new_message(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::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().20
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::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::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().20
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> > >::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::~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::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> > >::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::~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::Bool_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<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::Bool_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<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::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::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() 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::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() 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::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() 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::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() 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::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::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() 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::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::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::newMsg() 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::newMsg() 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::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::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::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::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::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::Bool_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<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::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const1
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()2
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()2
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const2
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const2
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const2
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const3
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::start()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().24
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::start()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl().24
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const4
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() 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::QuaternionStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)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::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()6
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&)6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().26
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::start()6
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&)6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().26
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const6
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().218
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().218
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().218
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().218
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const18
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().239
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().239
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const39
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const41
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const41
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const43
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()45
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&)45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().245
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()45
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&)45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().245
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const45
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const49
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const49
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().251
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().251
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().251
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().251
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const51
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&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().265
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&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().265
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const66
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()69
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()69
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()81
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&)81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().281
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()81
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&)81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().281
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const81
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const83
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()84
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&)84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().284
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()84
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&)84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().284
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()84
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().284
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()84
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().284
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const84
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()93
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&)93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().293
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()93
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&)93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().293
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const93
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()102
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&)102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()102
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&)102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const102
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()106
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()106
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const106
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const106
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()119
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&)119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()119
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&)119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const119
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()126
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()126
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const130
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()131
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&)131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()131
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&)131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2131
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()134
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()134
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const134
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const134
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const134
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()155
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()155
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const155
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()161
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&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()161
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&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const161
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()168
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&)168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()168
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&)168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const168
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const169
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()177
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()177
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const177
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const177
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const199
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const199
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&)208
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2208
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()210
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&)210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2210
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()211
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&)211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()211
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&)211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2211
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2250
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)251
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)251
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()252
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&)252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const252
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const259
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const271
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)514
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)514
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)565
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)565
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)565
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)565
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const606
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const606
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const606
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const606
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)724
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)747
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)1457
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)1457
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()1490
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1491
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()1492
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const1492
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2130
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const2130
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)3253
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)3256
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const4472
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const4472
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)5850
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)5850
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)5989
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)5989
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const10105
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const11569
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const11569
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const14974
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const14974
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()15846
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const15849
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const15851
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()15852
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()18987
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const18996
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const18997
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()18998
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const19525
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const19546
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)23854
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)23904
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)28223
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)28358
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const29392
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const29684
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)31942
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)31942
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)40692
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)40692
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const48007
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()48065
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()48065
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const48065
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const48065
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)48638
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)48638
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const65317
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const65317
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const70914
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const71345
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const75041
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const75041
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)94008
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)94017
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const94551
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const94554
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const95272
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const95338
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const98606
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const98609
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)99730
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)99747
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)102630
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)102632
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)104982
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)105012
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)107013
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)107183
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)116665
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)116752
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const123790
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()123908
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()123909
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const123925
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()140155
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()140158
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const140158
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const140158
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const141167
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const141256
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)162080
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)162218
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)180430
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)180501
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()208963
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const208969
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()208971
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const208972
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const260916
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const260917
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)302076
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)302128
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)456662
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)456745
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)529610
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)530065
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const1292197192
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()1292197193
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1472692191
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()1472692192
+
+
+ + + +
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..db3c08ad22 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,4024 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:43398643.9 %
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&)116752
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()161
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&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2161
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&)116665
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()161
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&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2161
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()84
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&)84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().284
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()84
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&)84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().284
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)99747
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()1490
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&)45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().245
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&)99730
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()45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()1492
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&)45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().245
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&)180501
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()69
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()134
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&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().265
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&)180430
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()69
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()134
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&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().265
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)302128
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()48065
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&)119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2119
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&)302076
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()119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()48065
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&)119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2119
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)530065
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()208963
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&)211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2211
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&)529610
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&)5
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()208971
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&)211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2211
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)456745
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()130
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&)102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2102
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&)456662
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()102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()130
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&)102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)105012
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()48007
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&)81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().281
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&)104982
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()81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()48007
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&)81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().281
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)107183
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()123908
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2155
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&)107013
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()155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()123909
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2155
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)94008
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()15846
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&)168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2168
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&)94017
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()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()15852
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&)168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2168
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)31942
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()177
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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&)31942
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()42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()177
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)48638
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().239
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&)48638
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()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().239
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)5850
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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&)5850
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()42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()0
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()0
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().20
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)1457
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()126
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2126
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&)1457
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()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()126
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2126
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().242
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)40692
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()2
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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&)40692
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()42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()2
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)28358
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()18987
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2250
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&)28223
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()252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()18998
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&)252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2252
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()0
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)251
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().218
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&)251
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()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().218
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)565
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().251
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&)565
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()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().251
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)514
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()1
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().218
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&)514
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()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()1
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().218
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().242
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().242
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)23904
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()1472692192
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&)208
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2208
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&)23854
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()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()1292197193
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&)210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2210
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)565
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().251
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&)565
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()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().251
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)3253
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()83
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&)6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().26
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&)3256
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()6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()83
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&)6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().26
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)102632
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()106
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&)93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().293
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&)102630
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()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()106
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&)93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().293
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)162218
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()140155
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&)131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2131
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&)162080
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()131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()140158
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&)131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2131
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)747
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::start()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().24
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)724
mrs_lib::SubscribeHandler<std_msgs::Bool_<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::Bool_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::start()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl().24
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)5989
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()10105
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().284
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&)5989
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()84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()10105
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().284
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]() const161
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]() const84
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2130
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1491
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() const2130
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const1492
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]() const45
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() const11569
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const134
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() const11569
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const134
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]() const66
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const98606
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const48065
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() const98609
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const48065
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]() const119
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const260916
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const208969
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]() const49
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const260917
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const208972
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]() const271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const130
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() const78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const130
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]() const102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const48007
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() const48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const48007
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]() const81
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const95272
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const141256
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const123790
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() const95338
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const141167
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const123925
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]() const155
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const19546
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const15849
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() const19525
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const15851
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]() const168
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const177
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() const2
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const177
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]() const42
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]() const39
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() 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::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const0
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]() const42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() 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::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const0
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]() const42
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]() 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::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const199
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const126
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]() const43
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const199
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const126
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]() const169
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]() const42
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]() const42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const41
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const2
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() const41
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const2
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]() const42
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const29684
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const18996
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() const29392
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const18997
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]() const252
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]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const4472
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() const4472
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]() const18
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const606
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() const39
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const606
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]() const51
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const4
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1
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() const4
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const1
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]() const18
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]() const42
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const71345
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1472692191
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]() const49
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const70914
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const1292197192
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]() const259
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const606
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() const39
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const606
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]() const51
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const83
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() const83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const83
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]() const6
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const65317
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const75041
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const106
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() const65317
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const75041
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const106
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]() const93
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const94551
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const140158
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]() const3
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const94554
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const140158
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]() const134
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const4
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const14974
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const10105
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() const10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const14974
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const10105
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]() const84
+
+
+ + + +
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..2aaad9dde8 --- /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:2023-12-06 07:59:45Functions:43398643.9 %
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        2610 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29        2610 :         : m_nh(options.nh),
+      30        2610 :           m_topic_name(options.topic_name),
+      31        2610 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35        2610 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39        2610 :           m_queue_size(options.queue_size),
+      40        2618 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42        2610 :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      43             :       {
+      44             :         // initialize a new TimeoutManager if not provided by the user
+      45          11 :         if (!m_timeout_manager)
+      46          11 :           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          22 :         std::function<void(const ros::Time&)> timeout_mgr_callback;
+      50          11 :         if (options.timeout_callback)
+      51           1 :           timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      52             :         else
+      53          10 :           timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      54             : 
+      55             :         // register the timeout callback with the TimeoutManager
+      56          11 :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, timeout_mgr_callback);
+      57             :       }
+      58             : 
+      59        7826 :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      60        2610 :       if (m_node_name.empty())
+      61           1 :         ROS_INFO_STREAM(msg);
+      62             :       else
+      63        2615 :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      64        2610 :     }
+      65             :     //}
+      66             : 
+      67        2622 :     virtual ~Impl() = default;
+      68             : 
+      69             :   public:
+      70             :     /* getMsg() method //{ */
+      71  1473308509 :     virtual typename MessageType::ConstPtr getMsg()
+      72             :     {
+      73  1473308509 :       m_new_data = false;
+      74  1473308509 :       m_used_data = true;
+      75  1473308509 :       return peekMsg();
+      76             :     }
+      77             :     //}
+      78             : 
+      79             :     /* peekMsg() method //{ */
+      80  1473308523 :     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  1653803523 :       return m_latest_message;
+      87             :     }
+      88             :     //}
+      89             : 
+      90             :     /* hasMsg() method //{ */
+      91      732461 :     virtual bool hasMsg() const
+      92             :     {
+      93      732461 :       return m_got_data;
+      94             :     }
+      95             :     //}
+      96             : 
+      97             :     /* newMsg() method //{ */
+      98      141167 :     virtual bool newMsg() const
+      99             :     {
+     100      141167 :       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      170921 :     virtual ros::Time lastMsgTime() const
+     130             :     {
+     131      170921 :       return m_latest_message_time;
+     132             :     };
+     133             :     //}
+     134             : 
+     135             :     /* topicName() method //{ */
+     136        2766 :     virtual std::string topicName() const
+     137             :     {
+     138        2766 :       std::string ret = m_sub.getTopic();
+     139        2766 :       if (ret.empty())
+     140        2621 :         ret = m_nh.resolveName(m_topic_name);
+     141        2766 :       return ret;
+     142             :     }
+     143             :     //}
+     144             : 
+     145             :     /* start() method //{ */
+     146        2614 :     virtual void start()
+     147             :     {
+     148        2614 :       if (m_timeout_manager)
+     149          15 :         m_timeout_manager->start(m_timeout_id);
+     150        2614 :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     151        2614 :     }
+     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           5 :     void default_timeout_callback(const std::string& topic_name, const ros::Time& last_msg)
+     196             :     {
+     197           5 :       const ros::Duration since_msg = (ros::Time::now() - last_msg);
+     198           5 :       const auto n_pubs = m_sub.getNumPublishers();
+     199          15 :       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           5 :       if (m_node_name.empty())
+     202           0 :         ROS_WARN_STREAM(txt);
+     203             :       else
+     204           5 :         ROS_WARN_STREAM("[" << m_node_name << "]: " << txt);
+     205           5 :     }
+     206             :     //}
+     207             : 
+     208             :     /* process_new_message() method //{ */
+     209     2448415 :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     210             :     {
+     211     2448415 :       m_latest_message_time = ros::Time::now();
+     212     2450310 :       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     2449671 :       m_new_data = !m_message_callback;
+     216     2447888 :       m_got_data = true;
+     217     2447888 :       m_new_data_cv.notify_one();
+     218     2449955 :     }
+     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        2610 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     254        2610 :         : impl_class_t::Impl(options, message_callback)
+     255             :     {
+     256        2606 :     }
+     257             : 
+     258             :   public:
+     259      732466 :     virtual bool hasMsg() const override
+     260             :     {
+     261     1464180 :       std::lock_guard lck(m_mtx);
+     262     1464270 :       return impl_class_t::hasMsg();
+     263             :     }
+     264      141256 :     virtual bool newMsg() const override
+     265             :     {
+     266      282210 :       std::lock_guard lck(m_mtx);
+     267      282116 :       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  1473308477 :     virtual typename MessageType::ConstPtr getMsg() override
+     275             :     {
+     276  2766121701 :       std::lock_guard lck(m_mtx);
+     277  2946616534 :       return impl_class_t::getMsg();
+     278             :     }
+     279  1473308380 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     280             :     {
+     281  2766121509 :       std::lock_guard lck(m_mtx);
+     282  2946616463 :       return impl_class_t::peekMsg();
+     283             :     }
+     284      170855 :     virtual ros::Time lastMsgTime() const override
+     285             :     {
+     286      341726 :       std::lock_guard lck(m_mtx);
+     287      341674 :       return impl_class_t::lastMsgTime();
+     288             :     };
+     289         144 :     virtual std::string topicName() const override
+     290             :     {
+     291         289 :       std::lock_guard lck(m_mtx);
+     292         290 :       return impl_class_t::topicName();
+     293             :     };
+     294        2614 :     virtual void start() override
+     295             :     {
+     296        5224 :       std::lock_guard lck(m_mtx);
+     297        5228 :       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        5216 :     virtual ~ImplThreadsafe() override = default;
+     306             : 
+     307             :   protected:
+     308     2449716 :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     309             :     {
+     310             :       {
+     311     4897010 :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     312     2442572 :         if (this->m_timeout_manager)
+     313       13919 :           this->m_timeout_manager->reset(this->m_timeout_id);
+     314     2432076 :         impl_class_t::process_new_message(msg);
+     315             :       }
+     316             : 
+     317             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     318     2443591 :       if (this->m_message_callback)
+     319     1417099 :         impl_class_t::m_message_callback(msg);
+     320     2441741 :     }
+     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:2023-12-06 07:59:45Functions: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..591ba60e2c --- /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:2023-12-06 07:59:45Functions: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..2567f78558 --- /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:2023-12-06 07:59:45Functions: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..e081662a31 --- /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:2023-12-06 07:59:45Functions:394881.2 %
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<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&)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<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&)3738
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&)3738
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)3738
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&)7035
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)7035
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&)21480
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&)27686
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&)45352
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)45352
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&)125899
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&)132934
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&)132934
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&)132935
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)139970
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&)211594
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&)211664
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&)213684
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&)245778
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&)245856
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)245873
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&)245888
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&)245888
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)256975
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&)259037
+
+
+ + + +
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..35214e5c68 --- /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:2023-12-06 07:59:45Functions:394881.2 %
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&)213684
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&)125899
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&)245888
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&)259037
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&)132934
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&)245888
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&)45352
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&)7035
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&)211664
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&)211594
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&)132935
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&)132934
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&)245856
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&)245778
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&)3738
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&)3738
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)256975
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)139970
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)245873
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)3738
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)45352
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)7035
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&)21480
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&)0
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&)27686
+
+
+ + + +
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..2cde4f26b0 --- /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:2023-12-06 07:59:45Functions:394881.2 %
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      646556 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16      646556 :     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       52387 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34       52387 :     msg.header = header;
+      35       52387 :   }
+      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       52387 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50       52387 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53      104774 :       std_msgs::Header new_header = getHeader(what);
+      54       52387 :       new_header.frame_id = frame_id;
+      55       52387 :       setHeader(ret, new_header);
+      56             :     }
+      57       52387 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65      637902 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67     1275790 :     const std::string from_frame = frame_from(tf);
+      68     1275782 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70      637895 :     if (from_frame == to_frame)
+      71       52387 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73     1171019 :     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      339598 :       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      339597 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90         982 :         const std::optional<T> tmp = doTransform(what, tf);
+      91         492 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93         492 :         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      245915 :       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      585019 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115      585512 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119     1170950 :       T result;
+     120      585504 :       tf2::doTransform(what, result, tf);
+     121      585500 :       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      594044 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140     1188322 :     const std_msgs::Header orig_header = getHeader(what);
+     141     1188336 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145      594194 :   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     1188466 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149      594281 :     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     1188545 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156     1188534 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157     1188534 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160     1188549 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161      594284 :     if (!tf_opt.has_value())
+     162        5591 :       return std::nullopt;
+     163      588681 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166     1177368 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167      588687 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175       49184 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177       98368 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179       49184 :     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       98368 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186       98368 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187       98368 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189       49184 :     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..e0393a0bdb --- /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:2023-12-06 07:59:45Functions: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..b19a5239c8 --- /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:2023-12-06 07:59:45Functions: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..1e44d82404 --- /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:2023-12-06 07:59:45Functions: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/index-detail-sort-f.html b/mrs_lib/include/mrs_lib/index-detail-sort-f.html new file mode 100644 index 0000000000..c98c115330 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-f.html @@ -0,0 +1,426 @@ + + + + + + + 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:73896576.5 %
Date:2023-12-06 07:59:45Functions:750126759.2 %
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
msg_extractor.h +
23.2%23.2%
+ 		23.2 %23 / 9922.7 %10 / 44
<unnamed>23.2 %23 / 9922.7 %10 / 44
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
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
<unnamed>90.1 %100 / 11135.7 %20 / 56
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.8 %355 / 699
<unnamed>79.6 %39 / 4950.8 %355 / 699
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
<unnamed>81.5 %44 / 5461.0 %25 / 41
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
<unnamed>79.3 %23 / 2975.0 %9 / 12
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
mutex.h +
100.0%
+ 		100.0 %11 / 1181.5 %66 / 81
<unnamed>100.0 %11 / 1181.5 %66 / 81
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
<unnamed>88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %112 / 113
<unnamed>100.0 %4 / 499.1 %112 / 113
timeout_manager.h +
66.7%66.7%
+ 		66.7 %4 / 6-0 / 0
<unnamed>66.7 %4 / 6-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 %22 / 22
<unnamed>100.0 %3 / 3100.0 %22 / 22
+
+
+ + + + +
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..dcf87a25ae --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-l.html @@ -0,0 +1,426 @@ + + + + + + + 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:73896576.5 %
Date:2023-12-06 07:59:45Functions:750126759.2 %
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
msg_extractor.h +
23.2%23.2%
+ 		23.2 %23 / 9922.7 %10 / 44
<unnamed>23.2 %23 / 9922.7 %10 / 44
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
<unnamed>33.3 %2 / 633.3 %1 / 3
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
<unnamed>65.5 %57 / 8732.9 %28 / 85
timeout_manager.h +
66.7%66.7%
+ 		66.7 %4 / 6-0 / 0
<unnamed>66.7 %4 / 6-0 / 0
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
<unnamed>76.5 %104 / 136100.0 %4 / 4
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
<unnamed>79.3 %23 / 2975.0 %9 / 12
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.8 %355 / 699
<unnamed>79.6 %39 / 4950.8 %355 / 699
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
<unnamed>81.5 %44 / 5461.0 %25 / 41
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
<unnamed>88.3 %248 / 28197.2 %70 / 72
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
<unnamed>90.1 %100 / 11135.7 %20 / 56
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 %22 / 22
<unnamed>100.0 %3 / 3100.0 %22 / 22
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
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.1 %112 / 113
<unnamed>100.0 %4 / 499.1 %112 / 113
mutex.h +
100.0%
+ 		100.0 %11 / 1181.5 %66 / 81
<unnamed>100.0 %11 / 1181.5 %66 / 81
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..c225b5000c --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail.html @@ -0,0 +1,426 @@ + + + + + + + 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:73896576.5 %
Date:2023-12-06 07:59:45Functions:750126759.2 %
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 +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
<unnamed>79.3 %23 / 2975.0 %9 / 12
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 / 5461.0 %25 / 41
<unnamed>81.5 %44 / 5461.0 %25 / 41
msg_extractor.h +
23.2%23.2%
+ 		23.2 %23 / 9922.7 %10 / 44
<unnamed>23.2 %23 / 9922.7 %10 / 44
mutex.h +
100.0%
+ 		100.0 %11 / 1181.5 %66 / 81
<unnamed>100.0 %11 / 1181.5 %66 / 81
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
<unnamed>88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %112 / 113
<unnamed>100.0 %4 / 499.1 %112 / 113
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 / 11135.7 %20 / 56
<unnamed>90.1 %100 / 11135.7 %20 / 56
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 %22 / 22
<unnamed>100.0 %3 / 3100.0 %22 / 22
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.8 %355 / 699
<unnamed>79.6 %39 / 4950.8 %355 / 699
timeout_manager.h +
66.7%66.7%
+ 		66.7 %4 / 6-0 / 0
<unnamed>66.7 %4 / 6-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
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..18d1e8c365 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-f.html @@ -0,0 +1,282 @@ + + + + + + + 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:73896576.5 %
Date:2023-12-06 07:59:45Functions:750126759.2 %
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
msg_extractor.h +
23.2%23.2%
+ 		23.2 %23 / 9922.7 %10 / 44
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
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.8 %355 / 699
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
mutex.h +
100.0%
+ 		100.0 %11 / 1181.5 %66 / 81
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %112 / 113
timeout_manager.h +
66.7%66.7%
+ 		66.7 %4 / 6-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 %22 / 22
+
+
+ + + + +
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..b7e6bab9ca --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-l.html @@ -0,0 +1,282 @@ + + + + + + + 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:73896576.5 %
Date:2023-12-06 07:59:45Functions:750126759.2 %
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
msg_extractor.h +
23.2%23.2%
+ 		23.2 %23 / 9922.7 %10 / 44
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
timeout_manager.h +
66.7%66.7%
+ 		66.7 %4 / 6-0 / 0
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.8 %355 / 699
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %22 / 22
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %112 / 113
mutex.h +
100.0%
+ 		100.0 %11 / 1181.5 %66 / 81
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..849611c7ea --- /dev/null +++ b/mrs_lib/include/mrs_lib/index.html @@ -0,0 +1,282 @@ + + + + + + + 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:73896576.5 %
Date:2023-12-06 07:59:45Functions:750126759.2 %
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 +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
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 / 5461.0 %25 / 41
msg_extractor.h +
23.2%23.2%
+ 		23.2 %23 / 9922.7 %10 / 44
mutex.h +
100.0%
+ 		100.0 %11 / 1181.5 %66 / 81
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %112 / 113
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
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 %22 / 22
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.8 %355 / 699
timeout_manager.h +
66.7%66.7%
+ 		66.7 %4 / 6-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
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..9895a1ac4f --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html @@ -0,0 +1,244 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:254161.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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
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<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&)39
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&)74
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5856
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&) const5856
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&) const5856
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&) const5856
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&)15332
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)15332
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) const15332
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&)48466
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)48466
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) const48466
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&)89248
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) const89314
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)89369
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)102576
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&) const102576
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&) const102576
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&) const102576
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)281131
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&) const281398
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&) const281443
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&) const281462
+
+
+ + + +
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..28cf7c025d --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func.html @@ -0,0 +1,244 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:254161.0 %
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
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&)15332
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)15332
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5856
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&)89248
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)89369
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)281131
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&)74
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&)48466
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)48466
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)102576
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&)39
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) const15332
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&) const5856
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&) const5856
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&) const5856
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&) const281462
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&) const281443
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&) const281398
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) const89314
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&) const102576
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&) const102576
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&) const102576
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) const48466
+
+
+ + + +
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..bd1bba54d6 --- /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:2023-12-06 07:59:45Functions:254161.0 %
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         113 :     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      389830 :     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      389830 :       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      137780 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140      137780 :       statecov_t ret;
+     141      137622 :       ret.x = state_predict(A, sc.x, B, u);
+     142      137639 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143      137636 :       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      153167 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156      153167 :       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      153046 :     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      153046 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177      389563 :     static R_t invert_W(R_t W)
+     178             :     {
+     179      389563 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180      389723 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183       57322 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184       57322 :         W += 1e-9 * ident;
+     185       57322 :         qr.compute(W);
+     186       57322 :         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      388589 :       const R_t W_inv = qr.inverse();
+     193      779516 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198      389875 :     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      389875 :       const R_t W = H * sc.P * H.transpose() + R;
+     202      389523 :       const R_t W_inv = invert_W(W);
+     203      389777 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204      779338 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210      389894 :     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      389894 :       statecov_t ret;
+     214      389710 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215      389616 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216      389446 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217      778944 :       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       15332 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       15332 :       statecov_t ret;
+     319       15332 :       A_t A = m_generateA(dt);
+     320       15332 :       B_t B = m_generateB(dt);
+     321       15332 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       15332 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       30664 :       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..5a83f9c0f1 --- /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:239923.2 %
Date:2023-12-06 07:59:45Functions:104422.7 %
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::Reference_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<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::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)0
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > 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::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)433
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)70155
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)70155
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)70156
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)70156
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)70156
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)70156
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)70156
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)70156
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)70156
+
+
+ + + +
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..5cc7482184 --- /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:239923.2 %
Date:2023-12-06 07:59:45Functions:104422.7 %
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&)70156
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)70155
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&)70156
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)70155
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)70156
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)70156
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&)70156
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)0
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)70156
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&)70156
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&)433
+
+
+ + + +
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..7acb8a233a --- /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:239923.2 %
Date:2023-12-06 07:59:45Functions:104422.7 %
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       70156 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
+      30             : 
+      31       70156 :   double x = data.x;
+      32       70156 :   double y = data.y;
+      33       70156 :   double z = data.z;
+      34             : 
+      35      140311 :   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       70156 : std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
+      96             : 
+      97       70156 :   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       70155 : double getHeading(const geometry_msgs::Pose& data) {
+     124             : 
+     125       70155 :   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       70156 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
+     184             : 
+     185       70156 :   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       70156 : double getHeading(const geometry_msgs::PoseWithCovariance& data) {
+     212             : 
+     213       70156 :   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       70156 : std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
+     336             : 
+     337       70156 :   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       70156 : std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
+     348             : 
+     349       70156 :   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       70155 : double getHeading(const nav_msgs::Odometry& data) {
+     392             : 
+     393       70155 :   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       70156 : double getHeading(const nav_msgs::OdometryConstPtr& data) {
+     404             : 
+     405       70156 :   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         433 : geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
+     448             : 
+     449         433 :   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           0 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
+     480             : 
+     481           0 :   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           0 : std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
+     579             : 
+     580           0 :   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           0 : double getHeading(const mrs_msgs::Reference& data) {
+     607             : 
+     608           0 :   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           0 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
+     639             : 
+     640           0 :   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           0 : double getHeading(const mrs_msgs::ReferenceStamped& data) {
+     667             : 
+     668           0 :   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..c967260975487de8223ac6d2a1cdfdd7ac5c9c17 GIT binary patch literal 1284 zcmV+f1^fDmP)o6G8|QDw%obIDPSgz2TC8I!)j9eb=wn zHK6s~;t9L#);4d*4YoTDm%r`ur_&tQ_LoohF6?F^db;-Q<8dv$=ZesdXzTY&fiA=9 z2=-A!M1B4EK1QNB`WX3L_HSyuFbtgOO@FUsjam*wY$w}xZ?jXsPTR>Xw&@+Q_m9~i z#J=r%?+NJP`aJBR{5+q-=kxV&82*|+q&0sl$YX`M-Z7|;@9Q3(gWJ*lE6ovYaq(!2 zNkTlfuu5c%$N8{P`I+ZwKG`%kFQhfE56C=$j^J-qN3?NFY8s+CZLtVsWIS5VCd8xX zI%LP=ay(6e@i?w9*E^waTrf3jv!QzXax={l+jI%qQ*$#IQQDp+)00iC z+5<%P$1!S8bAQ-Vo8oA5VQG7s3{MTQYEN>mEqoKX2KX!|?~${oX+1Z;_8=T1_PMsF zD9tI3G#3`Or)iv@UwaUak$aj3-*?^9Y#No5i2EhMjvU+k)e;q?Ivdk64#uM4c7G9VRuNc zEsofxQ)y57`PzKJ5h)&FM-ke?{%SR5?YSm5tFKl|gQ)H0#fzG}+>CL|xE(_f%bKs< z14o8Frg$Xp5VrXL#-l8*0mmZrQCx0jh7k3!!J?Kv^|j9!?U5Ic@av5f6f)P2yj~{P u8s9ZHW3sO8j$pVtlfok|0d^V!4!?hU9APhw2@? + + + + + + 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:2023-12-06 07:59:45Functions:668181.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::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)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_msgs::TrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >&)1
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)2
mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)2
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> >&)39
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)42
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)84
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)84
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)234
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> >&)894
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&)1140
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&)2311
int mrs_lib::get_mutexed<int>(std::mutex&, int&)4174
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&)4828
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)4862
auto mrs_lib::set_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)4897
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&)5089
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&)7160
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)7480
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> >&)9652
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&)10231
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)13128
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)19129
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)21721
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&)21842
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)22051
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)22051
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> >&)24316
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&)25437
auto mrs_lib::set_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >, mrs_msgs::TrackerCommand_<std::allocator<void> >&)31927
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)32049
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> >&)43361
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)44579
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)46557
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&)48022
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>&)48466
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&)48653
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&)52163
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>&)65240
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>&)67889
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> >&)70101
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>&)70559
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)80705
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&)85626
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>&)89092
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&)92844
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&)95244
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&)98585
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> >&)104326
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&)144695
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> >&)147675
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&)151042
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&)151042
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)177111
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> > >&)201794
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)228319
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)254219
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)287578
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> >&)293649
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)361876
double mrs_lib::get_mutexed<double>(std::mutex&, double&)368840
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&)370375
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&)370783
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)527172
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&)739131
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&)745538
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&)775468
+
+
+ + + +
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..23b1cee6ec --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func.html @@ -0,0 +1,404 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:668181.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&)21842
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>&)67889
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&)95244
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>&)65240
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&)2311
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)22051
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)7480
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)13128
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&)7160
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&)739131
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&)48022
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&)144695
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&)48653
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&)85626
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&)52163
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&)98585
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&)745538
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&)775468
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&)5089
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&)92844
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)177111
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)228319
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)22051
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&)25437
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&)10231
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)46557
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)19129
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)2
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)254219
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>&)89092
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>&)48466
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>&)70559
mrs_msgs::TrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >&)1
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)234
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)0
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)80705
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> >&)21721
mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)2
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> >&)44579
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)287578
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&)370783
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&)151042
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> >&)24316
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> >&)70101
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> > >&)201794
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)4862
double mrs_lib::get_mutexed<double>(std::mutex&, double&)368840
int mrs_lib::get_mutexed<int>(std::mutex&, int&)4174
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)361876
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)32049
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&)4828
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&)1140
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)84
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)84
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)42
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> >&)43361
auto mrs_lib::set_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >, mrs_msgs::TrackerCommand_<std::allocator<void> >&)31927
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> >&)894
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> >&)147675
auto mrs_lib::set_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)4897
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> >&)39
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> >&)293649
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> >&)104326
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&)370375
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&)151042
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> >&)9652
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)527172
+
+
+ + + +
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..a94ef1da43 --- /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:2023-12-06 07:59:45Functions:668181.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      302345 : std::tuple<Args...> get_mutexed(std::mutex& mut, Args&... args) {
+      49             : 
+      50      604692 :   std::scoped_lock lock(mut);
+      51             : 
+      52      302347 :   std::tuple result = std::tuple(args...);
+      53             : 
+      54      604694 :   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     5819607 : T get_mutexed(std::mutex& mut, T& arg) {
+      69             : 
+      70     9567123 :   std::scoped_lock lock(mut);
+      71             : 
+      72    11638866 :   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     1727067 : auto set_mutexed(std::mutex& mut, const T what, T& where) {
+     117             : 
+     118     2914698 :   std::scoped_lock lock(mut);
+     119             : 
+     120     1724604 :   where = what;
+     121             : 
+     122     3448506 :   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..1e2e9b975b --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html @@ -0,0 +1,368 @@ + + + + + + + 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:24828188.3 %
Date:2023-12-06 07:59:45Functions:707297.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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&)0
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
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
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
void 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>&)1
void 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&)1
void mrs_lib::ParamLoader::loadMatrixStatic<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)1
void mrs_lib::ParamLoader::loadMatrixStatic<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)1
void 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)1
void 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, 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::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > 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
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<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::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::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::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, 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&)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<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)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
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
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::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)4
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_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
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
Eigen::Matrix<float, -1, -1, 0, -1, -1> 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)8
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&)27
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)52
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)62
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)125
Eigen::Matrix<double, -1, -1, 0, -1, -1> 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)127
void mrs_lib::ParamLoader::loadMatrixStatic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)168
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)168
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_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)168
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&)210
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&)252
Eigen::Matrix<double, -1, -1, 0, -1, -1> 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)297
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&)462
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)462
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> >&)462
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)708
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)728
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1070
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&)1453
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)1454
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> > > >&)1454
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)1627
mrs_lib::ParamLoader::loadedSuccessfully()1764
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)2096
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)2306
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)2306
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)2478
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)4519
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4548
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> >&)6606
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&)6844
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)6844
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)7291
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)7291
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)16288
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)16918
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)16918
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)35585
+
+
+ + + +
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..ad3b41e908 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func.html @@ -0,0 +1,368 @@ + + + + + + + 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:24828188.3 %
Date:2023-12-06 07:59:45Functions:707297.2 %
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&)210
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)62
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&)1453
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&)6844
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)7291
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&)252
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&)462
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)16918
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&)0
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)2306
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4548
Eigen::Matrix<double, -1, -1, 0, -1, -1> 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)297
Eigen::Matrix<float, -1, -1, 0, -1, -1> 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)8
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
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
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
void 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>&)1
void 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&)1
void mrs_lib::ParamLoader::loadMatrixStatic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)168
void mrs_lib::ParamLoader::loadMatrixStatic<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)1
void mrs_lib::ParamLoader::loadMatrixStatic<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)1
void 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)1
void 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&)2
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::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::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > 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
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)168
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<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)2
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)125
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
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<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::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
mrs_lib::ParamLoader::loadedSuccessfully()1764
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1070
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&)35585
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
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::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
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::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)4
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_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)168
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_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
Eigen::Matrix<double, -1, -1, 0, -1, -1> 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)127
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)6844
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)1454
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)462
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)7291
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)16918
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)2306
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)52
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> >&)6606
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&)27
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> > > >&)1454
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> >&)462
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)4519
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)2478
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)16288
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)2096
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)728
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)708
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)1627
+
+
+ + + +
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..abc504adbd --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html @@ -0,0 +1,1397 @@ + + + + + + + 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:24828188.3 %
Date:2023-12-06 07:59:45Functions:707297.2 %
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          62 :   void printError(const std::string& str)
+      77             :   {
+      78          62 :     if (m_node_name.empty())
+      79          20 :       ROS_ERROR_STREAM(str);
+      80             :     else
+      81          42 :       ROS_ERROR_STREAM("[" << m_node_name << "]: " << str);
+      82          62 :   }
+      83           0 :   void print_warning(const std::string& str)
+      84             :   {
+      85           0 :     if (m_node_name.empty())
+      86           0 :       ROS_WARN_STREAM(str);
+      87             :     else
+      88           0 :       ROS_WARN_STREAM("[" << m_node_name << "]: " << str);
+      89           0 :   }
+      90             :   //}
+      91             : 
+      92             :   /* printValue function and overloads //{ */
+      93             : 
+      94             :   template <typename T>
+      95       33359 :   void printValue(const std::string& name, const T& value)
+      96             :   {
+      97       33359 :     if (m_node_name.empty())
+      98       18776 :       std::cout << "\t" << name << ":\t" << value << std::endl;
+      99             :     else
+     100       14614 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << value);
+     101       33359 :   }
+     102             : 
+     103             :   template <typename T>
+     104        1915 :   void printValue(const std::string& name, const std::vector<T>& value)
+     105             :   {
+     106        3830 :     std::stringstream strstr;
+     107        1915 :     if (m_node_name.empty())
+     108         934 :       strstr << "\t";
+     109        1915 :     strstr << name << ":\t";
+     110        1915 :     size_t it = 0;
+     111        6629 :     for (const auto& elem : value)
+     112             :     {
+     113        4714 :       strstr << elem;
+     114        4714 :       if (it < value.size() - 1)
+     115        3056 :         strstr << ", ";
+     116        4714 :       it++;
+     117             :     }
+     118        1915 :     if (m_node_name.empty())
+     119         934 :       std::cout << strstr.str() << std::endl;
+     120             :     else
+     121         981 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << strstr.str());
+     122        1915 :   }
+     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         252 :   void printValue(const std::string& name, const MatrixX<T>& value)
+     147             :   {
+     148         504 :     std::stringstream strstr;
+     149             :     /* const Eigen::IOFormat fmt(4, 0, ", ", "\n", "\t\t[", "]"); */
+     150             :     /* strstr << value.format(fmt); */
+     151         756 :     const Eigen::IOFormat fmt;
+     152         252 :     strstr << value.format(fmt);
+     153         252 :     if (m_node_name.empty())
+     154          42 :       std::cout << "\t" << name << ":\t" << std::endl << strstr.str() << std::endl;
+     155             :     else
+     156         210 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':" << std::endl << strstr.str());
+     157         252 :   }
+     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          52 :   std::string resolved(const std::string& param_name)
+     209             :   {
+     210          52 :     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       35585 :   bool check_duplicit_loading(const std::string& name)
+     216             :   {
+     217       35585 :     if (m_loaded_params.count(name))
+     218             :     {
+     219          14 :       printError(std::string("Tried to load parameter ") + name + std::string(" twice"));
+     220          14 :       m_load_successful = false;
+     221          14 :       return true;
+     222             :     } else
+     223             :     {
+     224       35571 :       return false;
+     225             :     }
+     226             :   }
+     227             :   //}
+     228             : 
+     229             :   /* loadMatrixStatic_internal helper function for loading static Eigen matrices //{ */
+     230             :   template <int rows, int cols, typename T>
+     231           5 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic_internal(const std::string& name, const Eigen::Matrix<T, rows, cols>& default_value, optional_t optional, unique_t unique)
+     232             :   {
+     233          10 :     MatrixX<T> dynamic = loadMatrixX(name, MatrixX<T>(default_value), rows, cols, optional, unique, NO_SWAP);
+     234           5 :     if (dynamic.rows() == rows || dynamic.cols() == cols)
+     235           5 :       return dynamic;
+     236             :     else
+     237           0 :       return default_value;
+     238             :   }
+     239             :   //}
+     240             : 
+     241             :   /* helper functions for loading dynamic Eigen matrices //{ */
+     242             :   // loadMatrixX helper function for loading dynamic Eigen matrices //{
+     243             :   template <typename T>
+     244         305 :   MatrixX<T> 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)
+     245             :   {
+     246         610 :     const std::string name_prefixed = m_prefix + name;
+     247         305 :     MatrixX<T> loaded = default_value;
+     248             :     // first, check if the user already tried to load this parameter
+     249         305 :     if (unique && check_duplicit_loading(name_prefixed))
+     250          12 :       return loaded;
+     251             : 
+     252             :     // this function only accepts dynamic columns (you can always transpose the matrix afterward)
+     253         293 :     if (rows < 0)
+     254             :     {
+     255             :       // if the parameter was compulsory, alert the user and set the flag
+     256           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed));
+     257           0 :       m_load_successful = false;
+     258           0 :       return loaded;
+     259             :     }
+     260         293 :     bool expect_zero_matrix = rows == 0;
+     261         293 :     if (expect_zero_matrix)
+     262             :     {
+     263           1 :       if (cols > 0)
+     264             :       {
+     265           0 :         printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed) + ". One dimension indicates zero matrix, but other expects non-zero.");
+     266           0 :         m_load_successful = false;
+     267           0 :         return loaded;
+     268             :       }
+     269             :     }
+     270             : 
+     271             : 
+     272         293 :     bool cur_load_successful = true;
+     273         293 :     bool check_size_exact = true;
+     274         293 :     if (cols <= 0)  // this means that the cols dimension is dynamic or a zero matrix is expected
+     275          82 :       check_size_exact = false;
+     276             : 
+     277         586 :     std::vector<T> tmp_vec;
+     278             :     // try to load the parameter
+     279         293 :     bool success = m_pp.getParam(name_prefixed, tmp_vec);
+     280             :     // check if the loaded vector has correct length
+     281         293 :     bool correct_size = (int)tmp_vec.size() == rows * cols;
+     282         293 :     if (!check_size_exact && !expect_zero_matrix)
+     283          81 :       correct_size = (int)tmp_vec.size() % rows == 0;  // if the cols dimension is dynamic, the size just has to be divisable by rows
+     284             : 
+     285         293 :     if (success && correct_size)
+     286             :     {
+     287             :       // if successfully loaded, everything is in order
+     288             :       // transform the vector to the matrix
+     289         251 :       if (cols <= 0 && rows > 0)
+     290          81 :         cols = tmp_vec.size() / rows;
+     291         251 :       if (swap)
+     292             :       {
+     293          39 :         int tmp = cols;
+     294          39 :         cols = rows;
+     295          39 :         rows = tmp;
+     296             :       }
+     297         251 :       loaded = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, Eigen::Unaligned>(tmp_vec.data(), rows, cols);
+     298             :     } else
+     299             :     {
+     300          42 :       if (success && !correct_size)
+     301             :       {
+     302             :         // warn the user that this parameter was not successfully loaded because of wrong vector length (might be an oversight)
+     303           0 :         std::string warning =
+     304             :             std::string("Matrix parameter ") + name_prefixed
+     305             :             + std::string(" could not be loaded because the vector has a wrong length " + std::to_string(tmp_vec.size()) + " instead of expected ");
+     306             :         // process the message correctly based on whether the loaded matrix should be dynamic or static
+     307           0 :         if (cols <= 0)  // for dynamic matrices
+     308           0 :           warning = warning + std::string("number divisible by ") + std::to_string(rows);
+     309             :         else  // for static matrices
+     310           0 :           warning = warning + std::to_string(rows * cols);
+     311           0 :         print_warning(warning);
+     312             :       }
+     313             :       // if it was not loaded, set the default value
+     314          42 :       loaded = default_value;
+     315          42 :       if (!optional)
+     316             :       {
+     317             :         // if the parameter was compulsory, alert the user and set the flag
+     318          42 :         printError(std::string("Could not load non-optional parameter ") + resolved(name_prefixed));
+     319          42 :         cur_load_successful = false;
+     320             :       }
+     321             :     }
+     322             : 
+     323             :     // check if load was a success
+     324         293 :     if (cur_load_successful)
+     325             :     {
+     326         251 :       if (m_print_values && printValues)
+     327         250 :         printValue(name_prefixed, loaded);
+     328         251 :       m_loaded_params.insert(name_prefixed);
+     329             :     } else
+     330             :     {
+     331          42 :       m_load_successful = false;
+     332             :     }
+     333             :     // finally, return the resulting value
+     334         293 :     return loaded;
+     335             :   }
+     336             :   //}
+     337             : 
+     338             :   /* loadMatrixArray_internal helper function for loading an array of EigenXd matrices with known dimensions //{ */
+     339             :   template <typename T>
+     340           5 :   std::vector<MatrixX<T>> loadMatrixArray_internal(const std::string& name, const std::vector<MatrixX<T>>& default_value, optional_t optional, unique_t unique)
+     341             :   {
+     342          10 :     const std::string name_prefixed = m_prefix + name;
+     343             :     int rows;
+     344          10 :     std::vector<int> cols;
+     345           5 :     bool success = true;
+     346           5 :     success = success && m_pp.getParam(name_prefixed + "/rows", rows);
+     347           5 :     success = success && m_pp.getParam(name_prefixed + "/cols", cols);
+     348             : 
+     349          10 :     std::vector<MatrixX<T>> loaded;
+     350           5 :     loaded.reserve(cols.size());
+     351             : 
+     352           5 :     int total_cols = 0;
+     353             :     /* check correctness of loaded parameters so far calculate the total dimension //{ */
+     354             : 
+     355           5 :     if (!success)
+     356             :     {
+     357           4 :       printError(std::string("Failed to load ") + resolved(name_prefixed) + std::string("/rows or ") + resolved(name_prefixed) + std::string("/cols"));
+     358           4 :       m_load_successful = false;
+     359           4 :       return default_value;
+     360             :     }
+     361           1 :     if (rows < 0)
+     362             :     {
+     363           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed) + std::string(" (rows and cols must be >= 0)"));
+     364           0 :       m_load_successful = false;
+     365           0 :       return default_value;
+     366             :     }
+     367           3 :     for (const auto& col : cols)
+     368             :     {
+     369           2 :       if (col < 0)
+     370             :       {
+     371           0 :         printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name_prefixed) + std::string(" (rows and cols must be >= 0)"));
+     372           0 :         m_load_successful = false;
+     373           0 :         return default_value;
+     374             :       }
+     375           2 :       total_cols += col;
+     376             :     }
+     377             :     
+     378             :     //}
+     379             : 
+     380           3 :     const MatrixX<T> loaded_matrix = loadMatrixX(name + "/data", MatrixX<T>(), rows, total_cols, optional, unique, NO_SWAP, false);
+     381             :     /* std::cout << "loaded_matrix: " << loaded_matrix << std::endl; */
+     382             :     /* std::cout << "loaded_matrix: " << loaded_matrix.rows() << "x" << loaded_matrix.cols() << std::endl; */
+     383             :     /* std::cout << "expected dims: " << rows << "x" << total_cols << std::endl; */
+     384           1 :     if (loaded_matrix.rows() != rows || loaded_matrix.cols() != total_cols)
+     385             :     {
+     386           0 :       m_load_successful = false;
+     387           0 :       return default_value;
+     388             :     }
+     389             : 
+     390           1 :     int cols_loaded = 0;
+     391           3 :     for (unsigned it = 0; it < cols.size(); it++)
+     392             :     {
+     393           2 :       const int cur_cols = cols.at(it);
+     394           2 :       const MatrixX<T> cur_mat = loaded_matrix.block(0, cols_loaded, rows, cur_cols);
+     395             :       /* std::cout << "cur_mat: " << cur_mat << std::endl; */
+     396           2 :       loaded.push_back(cur_mat);
+     397           2 :       cols_loaded += cur_cols;
+     398           2 :       printValue(name_prefixed + "/matrix#" + std::to_string(it), cur_mat);
+     399             :     }
+     400           1 :     return loaded;
+     401             :   }
+     402             :   //}
+     403             : 
+     404             :   /* loadMatrixStatic_internal helper function for loading EigenXd matrices with known dimensions //{ */
+     405             :   template <typename T>
+     406         172 :   MatrixX<T> loadMatrixStatic_internal(const std::string& name, const MatrixX<T>& default_value, int rows, int cols, optional_t optional, unique_t unique)
+     407             :   {
+     408         344 :     MatrixX<T> loaded = default_value;
+     409             :     // first, check that at least one dimension is set
+     410         172 :     if (rows <= 0 || cols <= 0)
+     411             :     {
+     412           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name) + std::string(" (use loadMatrixDynamic?)"));
+     413           0 :       m_load_successful = false;
+     414           0 :       return loaded;
+     415             :     }
+     416             : 
+     417         172 :     return loadMatrixX(name, default_value, rows, cols, optional, unique, NO_SWAP);
+     418             :   }
+     419             :   //}
+     420             : 
+     421             :   /* loadMatrixDynamic_internal helper function for loading Eigen matrices with one dynamic (unspecified) dimension //{ */
+     422             :   template <typename T>
+     423         127 :   MatrixX<T> loadMatrixDynamic_internal(const std::string& name, const MatrixX<T>& default_value, int rows, int cols, optional_t optional, unique_t unique)
+     424             :   {
+     425         254 :     MatrixX<T> loaded = default_value;
+     426             : 
+     427             :     // next, check that at least one dimension is set
+     428         127 :     if (rows <= 0 && cols <= 0)
+     429             :     {
+     430           0 :       printError(std::string("Invalid expected matrix dimensions for parameter ") + resolved(name) + std::string(" (at least one dimension must be specified)"));
+     431           0 :       m_load_successful = false;
+     432           0 :       return loaded;
+     433             :     }
+     434             : 
+     435         127 :     swap_t swap = NO_SWAP;
+     436         127 :     if (rows <= 0)
+     437             :     {
+     438          39 :       int tmp = rows;
+     439          39 :       rows = cols;
+     440          39 :       cols = tmp;
+     441          39 :       swap = SWAP;
+     442             :     }
+     443         127 :     return loadMatrixX(name, default_value, rows, cols, optional, unique, swap);
+     444             :   }
+     445             :   //}
+     446             :   //}
+     447             : 
+     448             :   /* load helper function for generic types //{ */
+     449             :   // This function tries to load a parameter with name 'name' and a default value.
+     450             :   // You can use the flag 'optional' to not throw a ROS_ERROR when the parameter
+     451             :   // cannot be loaded and the flag 'printValues' to set whether the loaded
+     452             :   // value and name of the parameter should be printed to cout.
+     453             :   // If 'optional' is set to false and the parameter could not be loaded,
+     454             :   // the flag 'load_successful' is set to false and a ROS_ERROR message
+     455             :   // is printer.
+     456             :   // If 'unique' flag is set to false then the parameter is not checked
+     457             :   // for being loaded twice.
+     458             :   // Returns a tuple, containing either the loaded or the default value and a bool,
+     459             :   // indicating if the value was loaded (true) or the default value was used (false).
+     460             :   template <typename T>
+     461       35281 :   std::pair<T, bool> load(const std::string& name, const T& default_value, optional_t optional = OPTIONAL, unique_t unique = UNIQUE)
+     462             :   {
+     463       70562 :     const std::string name_prefixed = m_prefix + name;
+     464       44047 :     T loaded = default_value;
+     465       35281 :     if (unique && check_duplicit_loading(name_prefixed))
+     466           2 :       return {loaded, false};
+     467             : 
+     468       35279 :     bool cur_load_successful = true;
+     469             :     // try to load the parameter
+     470       35279 :     const bool success = m_pp.getParam(name_prefixed, loaded);
+     471       35279 :     if (!success)
+     472             :     {
+     473             :       // if it was not loaded, set the default value
+     474         641 :       loaded = default_value;
+     475         641 :       if (!optional)
+     476             :       {
+     477             :         // if the parameter was compulsory, alert the user and set the flag
+     478           2 :         printError(std::string("Could not load non-optional parameter ") + resolved(name_prefixed));
+     479           2 :         cur_load_successful = false;
+     480             :       }
+     481             :     }
+     482             : 
+     483       35224 :     if (cur_load_successful)
+     484             :     {
+     485             :       // everything is fine and just print the name_prefixed and value if required
+     486       35277 :       if (m_print_values)
+     487       35277 :         printValue(name_prefixed, loaded);
+     488             :       // mark the param name_prefixed as successfully loaded
+     489       35332 :       m_loaded_params.insert(name_prefixed);
+     490             :     } else
+     491             :     {
+     492           2 :       m_load_successful = false;
+     493             :     }
+     494             :     // finally, return the resulting value
+     495       35279 :     return {loaded, success};
+     496             :   }
+     497             :   //}
+     498             : 
+     499             : public:
+     500             :   /*!
+     501             :     * \brief Main constructor.
+     502             :     *
+     503             :     * \param nh            The parameters will be loaded from rosparam using this node handle.
+     504             :     * \param printValues  If true, the loaded values will be printed to stdout using std::cout or ROS_INFO if node_name is not empty.
+     505             :     * \param node_name     Optional node name used when printing the loaded values or loading errors.
+     506             :     */
+     507        1627 :   ParamLoader(const ros::NodeHandle& nh, bool printValues = true, std::string_view node_name = std::string())
+     508        1627 :       : m_load_successful(true),
+     509             :         m_print_values(printValues),
+     510             :         m_node_name(node_name),
+     511             :         m_nh(nh),
+     512        1711 :         m_pp(nh, m_node_name)
+     513             :   {
+     514             :     /* std::cout << "Initialized1 ParamLoader for node " << node_name << std::endl; */
+     515        1627 :   }
+     516             : 
+     517             :   /* Constructor overloads //{ */
+     518             :   /*!
+     519             :     * \brief Convenience overload to enable writing ParamLoader pl(nh, node_name);
+     520             :     *
+     521             :     * \param nh            The parameters will be loaded from rosparam using this node handle.
+     522             :     * \param node_name     Optional node name used when printing the loaded values or loading errors.
+     523             :     */
+     524         708 :   ParamLoader(const ros::NodeHandle& nh, std::string_view node_name)
+     525         708 :       : ParamLoader(nh, true, node_name)
+     526             :   {
+     527             :     /* std::cout << "Initialized2 ParamLoader for node " << node_name << std::endl; */
+     528         708 :   }
+     529             : 
+     530             :   /*!
+     531             :     * \brief Convenience overload to enable writing ParamLoader pl(nh, "node_name");
+     532             :     *
+     533             :     * \param nh            The parameters will be loaded from rosparam using this node handle.
+     534             :     * \param node_name     Optional node name used when printing the loaded values or loading errors.
+     535             :     */
+     536             :   ParamLoader(const std::string& filepath, const ros::NodeHandle& nh)
+     537             :     : ParamLoader(nh, "none")
+     538             :   {
+     539             :     YAML::Node node = YAML::Load(filepath);
+     540             :   }
+     541             :   //}
+     542             : 
+     543             :   /* setPrefix function //{ */
+     544             :   
+     545             :   /*!
+     546             :     * \brief All loaded parameters will be prefixed with this string.
+     547             :     *
+     548             :     * \param prefix  the prefix to be applied to all loaded parameters from now on.
+     549             :     */
+     550         728 :   void setPrefix(const std::string& prefix)
+     551             :   {
+     552         728 :     m_prefix = prefix;
+     553         728 :   }
+     554             :   
+     555             :   //}
+     556             : 
+     557             :   /* getPrefix function //{ */
+     558             :   
+     559             :   /*!
+     560             :     * \brief Returns the current parameter name prefix.
+     561             :     *
+     562             :     * \return the current prefix to be applied to the loaded parameters.
+     563             :     */
+     564             :   std::string getPrefix()
+     565             :   {
+     566             :     return m_prefix;
+     567             :   }
+     568             :   
+     569             :   //}
+     570             : 
+     571             :   /* addYamlFile() function //{ */
+     572             :   
+     573             :   /*!
+     574             :     * \brief Adds the specified file as a source of static parameters.
+     575             :     *
+     576             :     * \param filepath The full path to the yaml file to be loaded.
+     577             :     * \return true if loading and parsing the file was successful, false otherwise.
+     578             :     */
+     579        4632 :   bool addYamlFile(const std::string& filepath)
+     580             :   {
+     581        4632 :     return m_pp.addYamlFile(filepath);
+     582             :   }
+     583             :   //}
+     584             : 
+     585             :   /* addYamlFileFromParam() function //{ */
+     586             :   
+     587             :   /*!
+     588             :     * \brief Loads a filepath from a parameter loads that file as a YAML.
+     589             :     *
+     590             :     * \param param_name Name of the parameter from which to load the YAML filename to be loaded.
+     591             :     * \return true      if loading and parsing the file was successful, false otherwise.
+     592             :     */
+     593        1070 :   bool addYamlFileFromParam(const std::string& param_name)
+     594             :   {
+     595        2140 :     std::string filepath;
+     596        1070 :     if (!loadParam(param_name, filepath))
+     597           0 :       return false;
+     598        1070 :     return m_pp.addYamlFile(filepath);
+     599             :   }
+     600             :   //}
+     601             : 
+     602             :   /* loadedSuccessfully function //{ */
+     603             :   /*!
+     604             :     * \brief Indicates whether all compulsory parameters were successfully loaded.
+     605             :     *
+     606             :     * \return false if any compulsory parameter was not loaded (is not present at rosparam server). Otherwise returns true.
+     607             :     */
+     608        1806 :   bool loadedSuccessfully()
+     609             :   {
+     610        1806 :     return m_load_successful;
+     611             :   }
+     612             :   //}
+     613             : 
+     614             :   /* loadParam function for optional parameters //{ */
+     615             :   /*!
+     616             :     * \brief Loads a parameter from the rosparam server with a default value.
+     617             :     *
+     618             :     * 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),
+     619             :     * the default value is used.
+     620             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     621             :     *
+     622             :     * \param name          Name of the parameter in the rosparam server.
+     623             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     624             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     625             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     626             :     */
+     627             :   template <typename T>
+     628        2507 :   bool loadParam(const std::string& name, T& out_value, const T& default_value)
+     629             :   {
+     630        2507 :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     631        2507 :     out_value = ret;
+     632        2536 :     return success;
+     633             :   }
+     634             :   /*!
+     635             :     * \brief Loads a parameter from the rosparam server with a default value.
+     636             :     *
+     637             :     * 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),
+     638             :     * the default value is used.
+     639             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     640             :     *
+     641             :     * \param name          Name of the parameter in the rosparam server.
+     642             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     643             :     * \return              The loaded parameter value.
+     644             :     */
+     645             :   template <typename T>
+     646         211 :   T loadParam2(const std::string& name, const T& default_value)
+     647             :   {
+     648         380 :     const auto loaded = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     649         380 :     return loaded.first;
+     650             :   }
+     651             :   /*!
+     652             :     * \brief Loads a parameter from the rosparam server with a default value.
+     653             :     *
+     654             :     * 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),
+     655             :     * the default value is used.
+     656             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     657             :     *
+     658             :     * \param name          Name of the parameter in the rosparam server.
+     659             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     660             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     661             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     662             :     */
+     663             :   template <typename T>
+     664             :   bool loadParamReusable(const std::string& name, T& out_value, const T& default_value)
+     665             :   {
+     666             :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, REUSABLE);
+     667             :     out_value = ret;
+     668             :     return success;
+     669             :   }
+     670             :   /*!
+     671             :     * \brief Loads an optional reusable parameter from the rosparam server.
+     672             :     *
+     673             :     * 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),
+     674             :     * the default value is used.
+     675             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     676             :     *
+     677             :     * \param name          Name of the parameter in the rosparam server.
+     678             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     679             :     * \return              The loaded parameter value.
+     680             :     */
+     681             :   template <typename T>
+     682             :   T loadParamReusable2(const std::string& name, const T& default_value)
+     683             :   {
+     684             :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, REUSABLE);
+     685             :     return ret;
+     686             :   }
+     687             :   //}
+     688             : 
+     689             :   /* loadParam function for compulsory parameters //{ */
+     690             :   /*!
+     691             :     * \brief Loads a compulsory parameter from the rosparam server.
+     692             :     *
+     693             :     * 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),
+     694             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     695             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     696             :     *
+     697             :     * \param name          Name of the parameter in the rosparam server.
+     698             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     699             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     700             :     */
+     701             :   template <typename T>
+     702       31428 :   bool loadParam(const std::string& name, T& out_value)
+     703             :   {
+     704       41087 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     705       32562 :     out_value = ret;
+     706       39953 :     return success;
+     707             :   }
+     708             :   /*!
+     709             :     * \brief Loads a compulsory parameter from the rosparam server.
+     710             :     *
+     711             :     * 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),
+     712             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     713             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     714             :     *
+     715             :     * \param name          Name of the parameter in the rosparam server.
+     716             :     * \return              The loaded parameter value.
+     717             :     */
+     718             :   template <typename T>
+     719             :   T loadParam2(const std::string& name)
+     720             :   {
+     721             :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     722             :     return ret;
+     723             :   }
+     724             :   /*!
+     725             :     * \brief Loads a compulsory parameter from the rosparam server.
+     726             :     *
+     727             :     * 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),
+     728             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     729             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     730             :     *
+     731             :     * \param name          Name of the parameter in the rosparam server.
+     732             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     733             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     734             :     */
+     735             :   template <typename T>
+     736           1 :   bool loadParamReusable(const std::string& name, T& out_value)
+     737             :   {
+     738           2 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, REUSABLE);
+     739           1 :     out_value = ret;
+     740           2 :     return success;
+     741             :   }
+     742             :   /*!
+     743             :     * \brief Loads a compulsory parameter from the rosparam server.
+     744             :     *
+     745             :     * 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),
+     746             :     * the loading process is unsuccessful (loaded_successfully() will return false).
+     747             :     * Using this method, the parameter can be loaded multiple times using the same ParamLoader instance without error.
+     748             :     *
+     749             :     * \param name          Name of the parameter in the rosparam server.
+     750             :     * \return              The loaded parameter value.
+     751             :     */
+     752             :   template <typename T>
+     753             :   T loadParamReusable2(const std::string& name)
+     754             :   {
+     755             :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, REUSABLE);
+     756             :     return ret;
+     757             :   }
+     758             :   //}
+     759             : 
+     760             :   /* loadParam specializations for ros::Duration type //{ */
+     761             : 
+     762             :   /*!
+     763             :     * \brief An overload for loading ros::Duration.
+     764             :     *
+     765             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+     766             :     *
+     767             :     * \param name          Name of the parameter in the rosparam server.
+     768             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     769             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     770             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     771             :     */
+     772             :   bool loadParam(const std::string& name, ros::Duration& out, const ros::Duration& default_value)
+     773             :   {
+     774             :     double secs;
+     775             :     const bool ret = loadParam<double>(name, secs, default_value.toSec());
+     776             :     out = ros::Duration(secs);
+     777             :     return ret;
+     778             :   }
+     779             : 
+     780             :   /*!
+     781             :     * \brief An overload for loading ros::Duration.
+     782             :     *
+     783             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+     784             :     *
+     785             :     * \param name          Name of the parameter in the rosparam server.
+     786             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     787             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     788             :     */
+     789             :   bool loadParam(const std::string& name, ros::Duration& out)
+     790             :   {
+     791             :     double secs;
+     792             :     const bool ret = loadParam<double>(name, secs);
+     793             :     out = ros::Duration(secs);
+     794             :     return ret;
+     795             :   }
+     796             :   
+     797             :   //}
+     798             : 
+     799             :   /* loadParam specializations for std_msgs::ColorRGBA type //{ */
+     800             : 
+     801             :   /*!
+     802             :     * \brief An overload for loading std_msgs::ColorRGBA.
+     803             :     *
+     804             :     * The color will be loaded as several \p double -typed variables, representing the R, G, B and A color elements.
+     805             :     *
+     806             :     * \param name          Name of the parameter in the rosparam server.
+     807             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     808             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     809             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     810             :     */
+     811             :   bool loadParam(const std::string& name, std_msgs::ColorRGBA& out, const std_msgs::ColorRGBA& default_value = {})
+     812             :   {
+     813             :     std_msgs::ColorRGBA res;
+     814             :     bool ret = true;
+     815             :     ret = ret & loadParam(name+"/r", res.r, default_value.r);
+     816             :     ret = ret & loadParam(name+"/g", res.g, default_value.g);
+     817             :     ret = ret & loadParam(name+"/b", res.b, default_value.b);
+     818             :     ret = ret & loadParam(name+"/a", res.a, default_value.a);
+     819             :     if (ret)
+     820             :       out = res;
+     821             :     return ret;
+     822             :   }
+     823             : 
+     824             :   /*!
+     825             :     * \brief An overload for loading std_msgs::ColorRGBA.
+     826             :     *
+     827             :     * The color will be loaded as several \p double -typed variables, representing the R, G, B and A color elements.
+     828             :     *
+     829             :     * \param name          Name of the parameter in the rosparam server.
+     830             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     831             :     * \return              The loaded parameter value.
+     832             :     */
+     833             :   std_msgs::ColorRGBA loadParam2(const std::string& name, const std_msgs::ColorRGBA& default_value = {})
+     834             :   {
+     835             :     std_msgs::ColorRGBA ret;
+     836             :     loadParam(name, ret, default_value);
+     837             :     return ret;
+     838             :   }
+     839             : 
+     840             :   //}
+     841             : 
+     842             :   /* loadParam specializations for XmlRpc::Value type //{ */
+     843             : 
+     844             :   /*!
+     845             :     * \brief An overload for loading an optional XmlRpc::XmlRpcValue.
+     846             :     *
+     847             :     * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+     848             :     * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+     849             :     * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+     850             :     *
+     851             :     * \param name          Name of the parameter in the rosparam server.
+     852             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     853             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     854             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     855             :     */
+     856           2 :   bool loadParam(const std::string& name, XmlRpc::XmlRpcValue& out, const XmlRpc::XmlRpcValue& default_value)
+     857             :   {
+     858           2 :     return loadParam<XmlRpc::XmlRpcValue>(name, out, default_value);
+     859             :   }
+     860             : 
+     861             :   /*!
+     862             :     * \brief An overload for loading a compulsory XmlRpc::XmlRpcValue.
+     863             :     *
+     864             :     * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+     865             :     * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+     866             :     * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+     867             :     *
+     868             :     * \param name          Name of the parameter in the rosparam server.
+     869             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     870             :     * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+     871             :     */
+     872           3 :   bool loadParam(const std::string& name, XmlRpc::XmlRpcValue& out)
+     873             :   {
+     874           3 :     return loadParam<XmlRpc::XmlRpcValue>(name, out);
+     875             :   }
+     876             : 
+     877             :   /*!
+     878             :     * \brief An overload for loading an optional XmlRpc::XmlRpcValue.
+     879             :     *
+     880             :     * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+     881             :     * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+     882             :     * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+     883             :     *
+     884             :     * \param name          Name of the parameter in the rosparam server.
+     885             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     886             :     * \return              the loaded parameter.
+     887             :     */
+     888           1 :   XmlRpc::XmlRpcValue loadParam2(const std::string& name, const XmlRpc::XmlRpcValue& default_value)
+     889             :   {
+     890           1 :     return loadParam2<XmlRpc::XmlRpcValue>(name, default_value);
+     891             :   }
+     892             : 
+     893             :   //}
+     894             : 
+     895             :   /* loadParam specializations and convenience functions for Eigen dynamic matrix type //{ */
+     896             : 
+     897             :   /*!
+     898             :     * \brief An overload for loading Eigen matrices.
+     899             :     *
+     900             :     * For compulsory Eigen matrices, use loadMatrixStatic() or loadMatrixDynamic().
+     901             :     * Matrix dimensions are deduced from the provided default value.
+     902             :     * 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),
+     903             :     * the default value is used.
+     904             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     905             :     *
+     906             :     * \param name          Name of the parameter in the rosparam server.
+     907             :     * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     908             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     909             :     */
+     910             :   template <typename T>
+     911             :   void loadParam(const std::string& name, MatrixX<T>& mat, const MatrixX<T>& default_value)
+     912             :   {
+     913             :     mat = loadParam2(name, default_value);
+     914             :   }
+     915             : 
+     916             :   /*!
+     917             :     * \brief An overload for loading Eigen matrices.
+     918             :     *
+     919             :     * For compulsory Eigen matrices, use loadMatrixStatic() or loadMatrixDynamic().
+     920             :     * Matrix dimensions are deduced from the provided default value.
+     921             :     * 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),
+     922             :     * the default value is used.
+     923             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     924             :     *
+     925             :     * \param name          Name of the parameter in the rosparam server.
+     926             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     927             :     * \return              The loaded parameter value.
+     928             :     */
+     929             :   template <typename T>
+     930             :   MatrixX<T> loadParam2(const std::string& name, const MatrixX<T>& default_value)
+     931             :   {
+     932             :     int rows = default_value.rows();
+     933             :     int cols = default_value.cols();
+     934             :     MatrixX<T> loaded;
+     935             :     loadMatrixDynamic(name, loaded, default_value, rows, cols);
+     936             :     return loaded;
+     937             :   }
+     938             :   
+     939             :   //}
+     940             : 
+     941             :   // loadMatrixStatic function for loading of static Eigen::Matrices //{
+     942             : 
+     943             :   /*!
+     944             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+     945             :     *
+     946             :     * 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).
+     947             :     * 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),
+     948             :     * the loading process is unsuccessful.
+     949             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     950             :     *
+     951             :     * \tparam rows  Expected number of rows of the matrix.
+     952             :     * \tparam cols  Expected number of columns of the matrix.
+     953             :     *
+     954             :     * \param name  Name of the parameter in the rosparam server.
+     955             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     956             :     *
+     957             :     */
+     958             :   template <int rows, int cols, typename T>
+     959           1 :   void loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat)
+     960             :   {
+     961           1 :     mat = loadMatrixStatic2<rows, cols, T>(name);
+     962           1 :   }
+     963             : 
+     964             :   /*!
+     965             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+     966             :     *
+     967             :     * 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).
+     968             :     * 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),
+     969             :     * the default value is used.
+     970             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     971             :     *
+     972             :     * \tparam rows          Expected number of rows of the matrix.
+     973             :     * \tparam cols          Expected number of columns of the matrix.
+     974             :     *
+     975             :     * \param name          Name of the parameter in the rosparam server.
+     976             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+     977             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+     978             :     *
+     979             :     */
+     980             :   template <int rows, int cols, typename T, typename Derived>
+     981           1 :   void loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat, const Eigen::MatrixBase<Derived>& default_value)
+     982             :   {
+     983           1 :     mat = loadMatrixStatic2<rows, cols, T>(name, default_value);
+     984           1 :   }
+     985             : 
+     986             :   /*!
+     987             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+     988             :     *
+     989             :     * 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).
+     990             :     * 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),
+     991             :     * the loading process is unsuccessful.
+     992             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+     993             :     *
+     994             :     * \tparam rows  Expected number of rows of the matrix.
+     995             :     * \tparam cols  Expected number of columns of the matrix.
+     996             :     *
+     997             :     * \param name  Name of the parameter in the rosparam server.
+     998             :     * \return      The loaded parameter value.
+     999             :     *
+    1000             :     */
+    1001             :   template <int rows, int cols, typename T = double>
+    1002           3 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic2(const std::string& name)
+    1003             :   {
+    1004           3 :     return loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>::Zero(), COMPULSORY, UNIQUE);
+    1005             :   }
+    1006             : 
+    1007             :   /*!
+    1008             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1009             :     *
+    1010             :     * 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).
+    1011             :     * 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),
+    1012             :     * the default value is used.
+    1013             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1014             :     *
+    1015             :     * \tparam rows          Expected number of rows of the matrix.
+    1016             :     * \tparam cols          Expected number of columns of the matrix.
+    1017             :     *
+    1018             :     * \param name          Name of the parameter in the rosparam server.
+    1019             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1020             :     * \return              The loaded parameter value.
+    1021             :     *
+    1022             :     */
+    1023             :   template <int rows, int cols, typename T, typename Derived>
+    1024           2 :   Eigen::Matrix<T, rows, cols> loadMatrixStatic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value)
+    1025             :   {
+    1026           2 :     return loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>(default_value), OPTIONAL, UNIQUE);
+    1027             :   }
+    1028             :   //}
+    1029             : 
+    1030             :   // loadMatrixStatic function for loading of Eigen matrices with known dimensions //{
+    1031             : 
+    1032             :   /*!
+    1033             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+    1034             :     *
+    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 loading process is unsuccessful.
+    1038             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1039             :     *
+    1040             :     * \param name  Name of the parameter in the rosparam server.
+    1041             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1042             :     * \param rows  Expected number of rows of the matrix.
+    1043             :     * \param cols  Expected number of columns of the matrix.
+    1044             :     */
+    1045             :   template <typename T>
+    1046         169 :   void loadMatrixStatic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1047             :   {
+    1048         169 :     mat = loadMatrixStatic2<T>(name, rows, cols);
+    1049         169 :   }
+    1050             : 
+    1051             :   /*!
+    1052             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1053             :     *
+    1054             :     * 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).
+    1055             :     * 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),
+    1056             :     * the default value is used.
+    1057             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1058             :     *
+    1059             :     * \param name          Name of the parameter in the rosparam server.
+    1060             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1061             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1062             :     * \param rows          Expected number of rows of the matrix.
+    1063             :     * \param cols          Expected number of columns of the matrix.
+    1064             :     */
+    1065             :   template <typename T, typename Derived>
+    1066           1 :   void loadMatrixStatic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1067             :   {
+    1068           1 :     mat = loadMatrixStatic2<T>(name, default_value, rows, cols);
+    1069           1 :   }
+    1070             : 
+    1071             :   /*!
+    1072             :     * \brief Specialized method for loading compulsory Eigen matrix parameters.
+    1073             :     *
+    1074             :     * 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).
+    1075             :     * 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),
+    1076             :     * the loading process is unsuccessful.
+    1077             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1078             :     *
+    1079             :     * \param name  Name of the parameter in the rosparam server.
+    1080             :     * \param rows  Expected number of rows of the matrix.
+    1081             :     * \param cols  Expected number of columns of the matrix.
+    1082             :     * \return      The loaded parameter value.
+    1083             :     */
+    1084             :   template <typename T = double>
+    1085         170 :   MatrixX<T> loadMatrixStatic2(const std::string& name, int rows, int cols)
+    1086             :   {
+    1087         170 :     return loadMatrixStatic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1088             :   }
+    1089             : 
+    1090             :   /*!
+    1091             :     * \brief Specialized method for loading Eigen matrix parameters with default value.
+    1092             :     *
+    1093             :     * 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).
+    1094             :     * 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),
+    1095             :     * the default value is used.
+    1096             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1097             :     *
+    1098             :     * \param name          Name of the parameter in the rosparam server.
+    1099             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1100             :     * \param rows          Expected number of rows of the matrix.
+    1101             :     * \param cols          Expected number of columns of the matrix.
+    1102             :     * \return              The loaded parameter value.
+    1103             :     */
+    1104             :   template <typename T, typename Derived>
+    1105           2 :   MatrixX<T> loadMatrixStatic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1106             :   {
+    1107           2 :     return loadMatrixStatic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1108             :   }
+    1109             :   //}
+    1110             : 
+    1111             :   // loadMatrixDynamic function for half-dynamic loading of MatrixX<T> //{
+    1112             : 
+    1113             :   /*!
+    1114             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1115             :     *
+    1116             :     * 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).
+    1117             :     * 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),
+    1118             :     * the loading process is unsuccessful.
+    1119             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1120             :     *
+    1121             :     * \param name  Name of the parameter in the rosparam server.
+    1122             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1123             :     * \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).
+    1124             :     * \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).
+    1125             :     */
+    1126             :   template <typename T>
+    1127           1 :   void loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1128             :   {
+    1129           1 :     mat = loadMatrixDynamic2<T>(name, rows, cols);
+    1130           1 :   }
+    1131             : 
+    1132             :   /*!
+    1133             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1134             :     *
+    1135             :     * 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).
+    1136             :     * 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),
+    1137             :     * the default value is used.
+    1138             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1139             :     *
+    1140             :     * \param name          Name of the parameter in the rosparam server.
+    1141             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1142             :     * \param mat           Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1143             :     * \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).
+    1144             :     * \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).
+    1145             :     */
+    1146             :   template <typename T, typename Derived>
+    1147           1 :   void loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1148             :   {
+    1149           1 :     mat = loadMatrixDynamic2<T>(name, default_value, rows, cols);
+    1150           1 :   }
+    1151             : 
+    1152             :   /*!
+    1153             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1154             :     *
+    1155             :     * 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).
+    1156             :     * 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),
+    1157             :     * the loading process is unsuccessful.
+    1158             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1159             :     *
+    1160             :     * \param name  Name of the parameter in the rosparam server.
+    1161             :     * \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).
+    1162             :     * \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).
+    1163             :     * \return      The loaded parameter value.
+    1164             :     */
+    1165             :   template <typename T = double>
+    1166         125 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, int rows, int cols)
+    1167             :   {
+    1168         125 :     return loadMatrixDynamic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1169             :   }
+    1170             : 
+    1171             :   /*!
+    1172             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1173             :     *
+    1174             :     * 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).
+    1175             :     * 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),
+    1176             :     * the default value is used.
+    1177             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1178             :     *
+    1179             :     * \param name          Name of the parameter in the rosparam server.
+    1180             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1181             :     * \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).
+    1182             :     * \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).
+    1183             :     * \return              The loaded parameter value.
+    1184             :     */
+    1185             :   template <typename T, typename Derived>
+    1186           2 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1187             :   {
+    1188           2 :     return loadMatrixDynamic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1189             :   }
+    1190             : 
+    1191             :   //}
+    1192             : 
+    1193             :   // loadMatrixArray function for loading of an array of MatrixX<T> with known dimensions //{
+    1194             :   /*!
+    1195             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1196             :     *
+    1197             :     * 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
+    1198             :     * 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
+    1199             :     * 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
+    1200             :     * \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.
+    1201             :     * A typical structure of a \c yaml file, specifying the
+    1202             :     * matrix array to be loaded using this method, is
+    1203             :     *
+    1204             :     * \code{.yaml}
+    1205             :     *
+    1206             :     * matrix_array:
+    1207             :     *   rows: 3
+    1208             :     *   cols: [1, 2]
+    1209             :     *   data: [-5.0, 0.0, 23.0,
+    1210             :     *          -5.0, 0.0, 12.0,
+    1211             :     *           2.0,   4.0,  7.0]
+    1212             :     *
+    1213             :     * \endcode
+    1214             :     *
+    1215             :     * 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.
+    1216             :     *
+    1217             :     * 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).
+    1218             :     * 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),
+    1219             :     * the loading process is unsuccessful.
+    1220             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1221             :     *
+    1222             :     * \param name  Name of the parameter in the rosparam server.
+    1223             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1224             :     *
+    1225             :     */
+    1226             :   template <typename T>
+    1227           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat)
+    1228             :   {
+    1229           1 :     mat = loadMatrixArray2<double>(name);
+    1230           1 :   }
+    1231             : 
+    1232             :   /*!
+    1233             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1234             :     *
+    1235             :     * 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
+    1236             :     * found in the \c rosparam server, instead of causing an unsuccessful load. This makes specifying the parameter value in the \c rosparam server optional.
+    1237             :     *
+    1238             :     * \param name           Name of the parameter in the rosparam server.
+    1239             :     * \param mat            Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1240             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1241             :     *
+    1242             :     */
+    1243             :   template <typename T>
+    1244           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat, const std::vector<MatrixX<T>>& default_value)
+    1245             :   {
+    1246           1 :     mat = loadMatrixArray2(name, default_value);
+    1247           1 :   }
+    1248             : 
+    1249             :   /*!
+    1250             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1251             :     *
+    1252             :     * This method works in the same way as the loadMatrixArray() method for compulsory parameters, except that the loaded
+    1253             :     * parameter is returned and not stored in the reference parameter.
+    1254             :     *
+    1255             :     * \param name           Name of the parameter in the rosparam server.
+    1256             :     * \returns              The loaded parameter or a default constructed object of the respective type.
+    1257             :     *
+    1258             :     */
+    1259             :   template <typename T = double>
+    1260           3 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name)
+    1261             :   {
+    1262           3 :     return loadMatrixArray_internal(name, std::vector<MatrixX<T>>(), COMPULSORY, UNIQUE);
+    1263             :   }
+    1264             : 
+    1265             :   /*!
+    1266             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1267             :     *
+    1268             :     * This method works in the same way as the loadMatrixArray() method for optional parameters, except that the loaded
+    1269             :     * parameter is returned and not stored in the reference parameter.
+    1270             :     *
+    1271             :     * \param name           Name of the parameter in the rosparam server.
+    1272             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1273             :     * \returns              The loaded parameter or the default value.
+    1274             :     *
+    1275             :     */
+    1276             :   template <typename T>
+    1277           2 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name, const std::vector<MatrixX<T>>& default_value)
+    1278             :   {
+    1279           2 :     return loadMatrixArray_internal(name, default_value, OPTIONAL, UNIQUE);
+    1280             :   }
+    1281             :   //}
+    1282             : 
+    1283             :   //}
+    1284             : 
+    1285             : };
+    1286             : //}
+    1287             : 
+    1288             :   /*!
+    1289             :     * \brief An overload for loading ros::Duration.
+    1290             :     *
+    1291             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1292             :     *
+    1293             :     * \param name          Name of the parameter in the rosparam server.
+    1294             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1295             :     * \return              The loaded parameter value.
+    1296             :     */
+    1297             :   template <>
+    1298             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name, const ros::Duration& default_value);
+    1299             : 
+    1300             :   /*!
+    1301             :     * \brief An overload for loading ros::Duration.
+    1302             :     *
+    1303             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1304             :     *
+    1305             :     * \param name          Name of the parameter in the rosparam server.
+    1306             :     * \return              The loaded parameter value.
+    1307             :     */
+    1308             :   template <>
+    1309             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name);
+    1310             : 
+    1311             : }  // namespace mrs_lib
+    1312             : 
+    1313             : #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..b4d6a7bfb0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html @@ -0,0 +1,349 @@ + + + + + + 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 + + +
+ 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..22a8302368250acc0e6662b3868567977bab6d44 GIT binary patch literal 4285 zcmV;u5JK;XP)wjsdp^?$tW|NWN;r}Phq>izIR*RKAe-VH~sCA2gh$2??+gu}rDp{3~o zH4%>57990Wclj2Cis6qg0NCE`^rh&4x0suiJN|JBG!kE^TT(e93-B`# zYi6GWE?@!BD9{#1s2_}4OUS_t#iQk@zP5AKrc{}5ODCr!Qx&0#(jak_Q!oxjH=0RF zX&a2Dqn5$wCMHG|LF3ugO=~aI-Dwn4ffb{$%oo4SN@djbxaZ(5uWhshbY%^RGPh-D#ZVn9G>T?!irV=7n@zu=NHM*t_r|q z<+#B61%@?S2BRc=T;O+lx1^>G0v8P1pR&$OVLzHplU&Dy%V-`~!UbirbeR4l)XB#( zm|HZPNtErbMo=V{bC+dmTG36*VC>2zes5bjQn^x}&Q&gm1)326hieq8P{J2P1Z)e~ zkzw~!GOZl4^r~n$(P6Qm{fS5zG#L_=C}N=7CbVT|SmRhrpS~5pQg%L#y2>m^P3yX; zPsJ@$IOTPXdSCQ$uZWnTP(ei@FDkO<XMX`V~E$Y_1jtr#+(rEpN4VqrE0 zbE8qLzXyFJ>(-`xJnkv2_0e(9yqsW~ZxRnYKR7LPodXIS;R=R1HCZ3b-5$=22~~4# ziBbf9T8!h&Xn3Y6ye}#2nwN!Sy3`1DQA%V<>M$h4WOSXJnFhCza5o$?1f|K{)5q?d z2tutKw=>g#nQdm~e)uS+a2yQl+O|(&IWtndS26HhtD1h|6e?_~bqdG1&k2K3hXB$@ z^_$k3X7$wy6~sMb)6$$QTVs(Cn%!_$8T4+rRceCwKUmQ^B||zoF3cUf(D_2sjgGxA zq;*0y9uFUNmH}YN*~{G1r}Smx6By_F#iJ08z4DRuapsVT)M$}}JvT#<)KDMRKB|<) z8LSyJJs7-RJchLJRG8s{zC;+@z~l^<`$IELqY0b~TU+R_ISyYx$+26;cL9zJTylr_ zX306C1yID{sy-%*`>j|m&=&VknrlepaXUfa2@g484Gp}T0*DP6yPPwbmr-cGUi;iI z+lvU07JA5AH#tJ)wmRaJ^O2>R`Ev-fZ9H1A7BCwQMmg#@HGx!7{_}Enu}(3F>rF;# zZ97Q(@a7-bfRa!lgAWWrl5YIKI$l(hZ%O#`(^-uZ$~DlLFQs<5ER$N2t!RixMvF(t z@ur-bl~BcU6^}~d8DUT2NH~=EN~n^!JEL5vyz;DhV<4o!MY7PT(V%kugG@3G(LQiM zb5E6yb#;%!nYkA*7^+;zaNxXBA7|)U#{$u)=iF0^nnyWmSQcB*MBW=5?=nAHpmMNm zIP%|Hdd6~`vx!`*N%JCPZ<6CITkR0q>?1l(Tu84juZS=nwgXlpDU>+KuG7aT@_7VZ zm7zfZ~)GMqJftRD=(2iz|1UiGx86jMbu_CL|u zcWwArRxe0Jg70#R3=E04G>M(VxK(#uR`U#Tu^rXZj@&?zW$NR1tWA7@m-?>7&knoz z^MgO~&f+Z?H8A_h2cY9v_{Gw@HU(+%g)Z&N_9P?_Qw2Rjulop}yLN0J%oO3??Y7PB z7qb6g*^_d~wV`%$Z|UO+OOSw5SRrna8ESjBurS%8rOkIE@TEbyExI&#o>n3m&9u$C zioKU;P#QC%Nx805+arrD9%=wfY0a+%AU2*3OC)FH*TeNp;z&Ij5ze)?svMUTg%;i& zmgDbcLfz$VrNrBgeWZUxsBe0KzK=pKu_%ejMNMZ#X`KP zwr8yj4?1e;iXAOXqlM>1wDXSxmoF6-LGV{;jqaI`Sl19C^93@7Uww_hTUjbR1?4v9L3e|+wqX*_3?Hap&S$b)&Om9YgU`@(>)@LrdK6CuCi<7njO|Cd<2#fLA`b>|vZGC1= zvw;>sOiMx7*JvK~8!qH*cA}I`KQH^3CM`>S(*a|Mk(0G?Ow=C1F3#x!$UAv(8O0Y9 zl3{2Om-mSIw5x8m)Qprx&!|Xx3aBKjrGZB6&aOEu)_`>() zpURne2~hkW=Q=xofmH%yg-H}&f}rdT%y{-gepYMs!%u&b1heKtI2vIl5<@=UGM@x1 za071A#Jw3=8!I=xrO3A&Hc~HZS#ch&EWE@+eiuMbv0bsIyC#m}$mQ$^R*0~mlS+ot z#P?g)cF_srdk{x;%crdZ9ETr!z)r=y5GL|SQov?B(k5Rh%>j;bLiB#3L|7a!>-HF_ ztU~aJNx93)P%{v}%o-8~2ah#kwmlx*6-N~aC17O_xxyqWL|DndDz78-xMH(5A7mWT zCd57H5i#+m@MT^(8&m*95l~{c_5ogO8Z0Hf8gK`&-4U=eBt*c2X2qF(A+JT$D!xk$Ows}=^Y-24!3&~AJBL) zXHR(~ErQwl7`wN=lHHQ7~s?g-x-hGeNR=mtPQ8Sc?odDxIGy*A>1GOZ83k40<>~~o$jNhFOg8s z(W)~vyK?$*51zY5+@o|i4Sj=9*>cm^duk>S z;nR%$tK#vB0sFHpZrAD_YH=gmkHvi#gkLJWt`I6)R#;qdQsnD|*JMc;bNe_NcQtXl zdAcJB^&A8CK7!19RE=qZXDIxRomX#Dbm4hMJ(%*Iqybp>Q>X}&JuIp@@+}Z{znZ&j z`bgzcUmt}3d6d!vt{jVY&<_K5+tOPBSIx2RKBoO1X%Kq5Tg8D+64T7bNg{0 zxfY7fSl!2jyoPP-zPCAqOcA~4WVi~U&hh|Dosqh}NqJ9oHEH2diOIM`KCCO{w zjeUFtw5Y3HY6lFk+o(uQhfu(ggsv!K;YV@ZAJ>4 z3QcpBZ`;WZ@RQl!YznXQ?$N0h7f^<fT&Ek+F zNv7(AtAusWesoQ$A869K%Qh{{0!?K1N%vmN7d|WJl1=yYra`#Xn;klXFRKC@>rA@& zpw6%l{3K^@=su403h>!+&Sh7H?)N)az6DCeZuH}bRH}>=KdIFFxR0lJ1@XDVmjOra zqpdSyYt=D2BmKxV5+8tQ;5Q!Kh>_zVo{PPgyN0@6-g7;a_&|@zK5hx6NP(sM%g58< z1$^)KBSBmdy%FJ6gSgA7!uxnGmpC@{rfj;YH*LZ~Z`|35PgQ5)%?EWx#Ci)6estzt z+{ck#0fM+IBtEhZfIl8ieE5tLQnM>2J{MJm_i-ObdWFE|{n?4n_Uoc;2=D2P^dqmd za~~y$wj6Oxpgc@n>8CRxoFF)PiEYpuPaG$VaS+^zHqZc9>O|=AZTvjIXyxz8+&k85 zQcf?_Ab?qjgfz~OPP+il0)O6!2m4OeU?UBaz3ZNI zdlB_>ltb=A+w?iv@HYI;wo@9>vyJR)E94i>hTj^GI{VlFE(yxq#}f?c0JujSwo&^+ zl;^I+9nDd_a(Db#{`Y4qV%n7zhA?q(XVrT&1DeP=Rjw@B(_a7MxxVrg8W!#Ym2{3W f1#9!|>E-J`6YuqhU_-h-00000NkvXXu0mjf4lXN) 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..fd8a1413f5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html @@ -0,0 +1,532 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:11211399.1 %
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<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()9
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()9
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()18
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()39
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()43
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()85
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()113
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()123
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()126
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()128
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()152
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()152
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()168
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()168
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()168
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()183
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()229
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()229
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()245
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()252
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()252
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()252
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()254
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()271
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()304
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()308
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()308
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()336
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()384
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()441
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()504
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()616
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()686
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()16917
+
+
+ + + +
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..2c02d33e75 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,532 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:11211399.1 %
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()168
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()336
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()123
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()252
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()504
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()441
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()686
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()152
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()304
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()308
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()616
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()271
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()384
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()85
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()128
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()229
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()229
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()126
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()84
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()168
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()254
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()16917
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()9
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()18
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()42
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()84
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()126
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()252
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()168
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()39
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()252
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()245
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()152
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()308
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()113
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()43
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()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()42
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()84
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()183
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()9
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()42
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()42
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()126
+
+
+ + + +
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..266f9bbc10 --- /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:2023-12-06 07:59:45Functions:11211399.1 %
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        2774 :   ~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        3807 :   PublisherHandler(void){};
+     102             : 
+     103             :   /**
+     104             :    * @brief generic destructor
+     105             :    */
+     106       23103 :   ~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          52 :   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..ebeff34eb1 --- /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:2023-12-06 07:59:45Functions: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)27769
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)34812
+
+
+ + + +
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..f21fbae26c --- /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:2023-12-06 07:59:45Functions: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)27769
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)34812
+
+
+ + + +
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..773a9eb600 --- /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:2023-12-06 07:59:45Functions: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       34812 : double inline throttleToForce(const MotorParams_t motor_params, const double throttle) {
+      20             : 
+      21       34812 :   return motor_params.n_motors * pow((throttle - motor_params.B) / motor_params.A, 2);
+      22             : }
+      23             : 
+      24       27769 : double inline forceToThrottle(const MotorParams_t motor_params, const double force) {
+      25             : 
+      26       27769 :   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..d993dd2040 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html @@ -0,0 +1,304 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:205635.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<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::LKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<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::LKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<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&)107
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&)189
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, 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>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t const&)303
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&)303
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&)606
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&)606
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)615
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&)2036
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&)5088
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&)14223
+
+
+ + + +
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..2db8d0b32e --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func.html @@ -0,0 +1,304 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:205635.7 %
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&)14223
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&)5088
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)615
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&)2036
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&)303
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&)606
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&)107
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&)303
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&)189
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&)606
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::LKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<2, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<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::LKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<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::LKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<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::LKF<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..1cb81a14d3 --- /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:2023-12-06 07:59:45Functions:205635.7 %
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       14121 :       do
+      81             :       {
+      82       14223 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       14223 :         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       14223 :         ros::Time next_stamp = to_stamp;
+      90       14223 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       14121 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       14223 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       14223 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       14223 :         hist_it++;
+      99       14223 :       } 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         606 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         606 :       assert(!m_history.empty());
+     120         606 :       const auto& info = m_history.front();
+     121         606 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         606 :       sc.stamp = to_stamp;
+     123         606 :       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        5188 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        4988 :           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         189 :     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         189 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         189 :       m_history.front().u = u;
+     172         189 :       m_history.front().Q = Q;
+     173         189 :       m_history.front().stamp = stamp;
+     174         189 :       m_history.front().predict_model = model;
+     175         189 :     }
+     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         107 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222         107 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224         107 :       m_history.front().u = u;
+     225         107 :       m_history.front().stamp = stamp;
+     226         107 :       m_history.front().predict_model = model;
+     227         107 :     }
+     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         303 :     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         303 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         303 :       auto& info = m_history.front();
+     327         303 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         303 :       const auto sc = predictTo(to_stamp);
+     329         303 :       info.z = z;
+     330         303 :       info.R = R;
+     331         303 :       info.stamp = to_stamp;
+     332         303 :       info.is_measurement = true;
+     333         303 :       info.meas_id = meas_id;
+     334         303 :       info.correct_model = model;
+     335         303 :       m_sc = correctFrom(sc, info);
+     336         303 :     }
+     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         617 :       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        5088 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        5088 :         u = info.u;
+     433        5088 :         Q = info.Q;
+     434        5088 :         predict_model = info.predict_model;
+     435        5088 :       };
+     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        2036 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2036 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       14829 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       29658 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       14829 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       29658 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5453 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5453 :       assert(meas.is_measurement);
+     544       10906 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5453 :       auto sc_tmp = sc;
+     546       10906 :       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..cd47c7d37b --- /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:2023-12-06 07:59:45Functions: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..3242a200a8 --- /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:2023-12-06 07:59:45Functions: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..8fac34b409 --- /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:2023-12-06 07:59:45Functions: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..9fb8de7f35 --- /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:2023-12-06 07:59:45Functions: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..90dc7c49f3 --- /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:2023-12-06 07:59:45Functions: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..94d5d4cb8d --- /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:2023-12-06 07:59:45Functions: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..f470496551 --- /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:2023-12-06 07:59:45Functions: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..b4c1ffde6a --- /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:2023-12-06 07:59:45Functions: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..0bdebdd6e5 --- /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:2023-12-06 07:59:45Functions: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..dec4d33efb --- /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:2023-12-06 07:59:45Functions: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..19905cabec --- /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:2023-12-06 07:59:45Functions: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..e7fa011705 --- /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:2023-12-06 07:59:45Functions: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..ad9174122b --- /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:2023-12-06 07:59:45Functions: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&)1768736
+
+
+ + + +
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..c5cce2d428 --- /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:2023-12-06 07:59:45Functions: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&)1768736
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..d39165ea24 --- /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:2023-12-06 07:59:45Functions: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     1768736 :     time_point(const std::string& label) : ros_time(ros::Time::now()), chrono_time(std::chrono::steady_clock::now()), label(label) {
+      93     1768493 :     }
+      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:2023-12-06 07:59:45Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler_impl<mrs_msgs::PathSrv>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::ServiceClientHandler()74
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::~ServiceClientHandler_impl()74
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()84
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()84
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()84
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()93
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()93
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::~ServiceClientHandler()148
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()186
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()325
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()327
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()555
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()555
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()650
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1066
+
+
+ + + +
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..ba63f277de --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html @@ -0,0 +1,168 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()42
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()84
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()42
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()84
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()42
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()84
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::ServiceClientHandler()74
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::~ServiceClientHandler()148
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()93
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()186
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()325
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()650
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()555
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1066
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()42
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::~ServiceClientHandler_impl()74
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()93
mrs_lib::ServiceClientHandler_impl<mrs_msgs::PathSrv>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()327
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()555
+
+
+ + + +
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..064b26a9d8 --- /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:2023-12-06 07:59:45Functions:2222100.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        1183 :   ~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        1175 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139        2314 :   ~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..a4f9550e12 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,2876 @@ + + + + + + + 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:394979.6 %
Date:2023-12-06 07:59:45Functions:35569950.8 %
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::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::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::HwApiRcChannels_<std::allocator<void> > >::getMsg()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::ObstacleSectors_<std::allocator<void> > >::getMsg()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> > >::start()0
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> > >&&)0
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&)0
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*)0
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*)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()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::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> > >::start()0
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> > >&&)0
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&)0
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*)0
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*)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()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::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::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::Bool_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::getMsg()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::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> > >::hasMsg() 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> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() 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::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> > >::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> > >::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> > >::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::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::Bool_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::topicName[abi:cxx11]() 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::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::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::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()() const0
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()() 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<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<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>*)2
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>*)2
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()2
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*)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>*)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>*)2
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*)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<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>*)2
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*)2
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*)2
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const2
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()() 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*)::{lambda()#1}::operator()() const2
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()() const2
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()() const2
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const3
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::start()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >&&)4
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{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::RtkGps_<std::allocator<void> > >::start()6
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&)6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)6
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::~SubscribeHandler()8
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*)9
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*)9
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9
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*)9
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*)9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9
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()() const9
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const9
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()() const9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const9
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)18
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()() const18
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>*)27
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>*)27
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()() const27
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()36
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()36
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>*)37
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>*)37
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>*)37
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>*)37
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()() const37
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()() const37
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&)39
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>*)39
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*)39
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>*)39
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*)39
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*)39
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*)39
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*)39
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*)39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)39
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const39
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()() const39
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()() const39
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()() const39
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()() const39
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()() const39
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()() const39
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()() const39
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*)40
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*)40
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()() const40
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const41
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*)42
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*)42
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*)42
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*)42
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&)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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*)42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
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()() const42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const43
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()45
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&)45
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>*)45
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>*)45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)45
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&)45
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()() const45
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()() const45
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const49
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const50
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)51
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()() const51
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()() const51
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()() const51
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&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()69
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()81
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&)81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)81
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()83
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const83
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()84
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&)84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)84
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()84
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()84
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const84
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()() const84
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&)91
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()() const91
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()93
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&)93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)93
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()102
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&)102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)102
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()102
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()102
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()106
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()119
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&)119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)119
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()126
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&)126
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&)126
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()() const126
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()() const126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()130
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()131
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&)131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)131
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()134
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()154
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()154
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()154
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()154
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()154
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()155
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()155
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)155
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()() const155
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()161
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&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()163
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()165
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()168
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&)168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)168
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()168
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()177
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()186
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()199
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const199
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()202
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()204
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()208
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()210
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&)210
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&)210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)210
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()() const210
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()211
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&)211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)211
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()220
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()232
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()250
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()252
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&)252
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&)252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)252
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()() const252
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()310
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()324
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()327
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()336
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()351
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()416
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()458
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()500
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()522
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const606
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const606
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()733
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()1061
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()1491
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const2130
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const4472
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const10105
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const11570
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const14974
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()15850
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()18989
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const19566
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const29689
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()48007
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const48007
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()48065
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const65317
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const71348
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const75041
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const94555
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const95215
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const98610
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()123900
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()140156
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const141294
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()208971
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const260937
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()1472692193
+
+
+ + + +
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..45169cf8ae --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,2876 @@ + + + + + + + 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:394979.6 %
Date:2023-12-06 07:59:45Functions:35569950.8 %
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()161
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&)161
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()163
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*)42
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*)42
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*)40
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>*)37
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*)42
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*)42
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*)40
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>*)37
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)161
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()84
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&)84
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()84
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&)42
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*)42
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*)42
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()168
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)84
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()45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()1491
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&)45
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()154
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>*)45
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>*)45
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()199
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)45
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()154
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()154
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()69
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()134
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&)65
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()155
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>*)27
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>*)37
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>*)27
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>*)37
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()220
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)65
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()119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()48065
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&)119
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()232
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&)39
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>*)39
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>*)2
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*)39
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>*)39
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>*)2
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*)39
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()351
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)119
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()211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()208971
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&)211
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()522
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&)91
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*)42
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*)39
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*)39
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*)42
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*)39
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*)39
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()733
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)211
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()154
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()154
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()102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()130
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&)102
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()102
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&)51
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*)42
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*)9
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*)42
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*)9
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()204
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)102
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()81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()48007
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&)81
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()84
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&)39
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()165
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)81
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()155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()123900
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()155
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&)155
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()310
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)155
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()168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()15850
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&)168
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()168
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&)126
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()336
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)168
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()42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()177
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)42
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()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()39
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&)39
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()78
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)39
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()42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()0
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)42
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()42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()0
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)42
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()0
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> > >&&)0
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&)0
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*)0
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*)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()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> > >::start()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)42
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()126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()126
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()84
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&)126
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()210
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)84
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)42
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)42
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()42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()2
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)42
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()252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()18989
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&)252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()250
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&)252
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()500
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)252
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()0
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> > >&&)0
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&)0
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*)0
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*)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()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> > >::start()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()18
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()36
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)18
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()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()102
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)51
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()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()1
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&)18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9
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*)9
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*)9
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()36
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)18
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()42
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&)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()84
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)42
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()210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()1472692193
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&)210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()208
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&)210
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()416
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)210
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()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()51
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&)51
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()102
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)51
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()6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()83
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&)6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()196
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*)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>*)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>*)2
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*)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<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>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()202
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)6
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()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()106
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&)93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()93
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9
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*)42
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*)42
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*)42
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*)42
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()186
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)93
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()131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()140156
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&)131
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()327
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*)2
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&)45
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*)2
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*)42
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*)42
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*)42
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*)42
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()458
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)131
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::start()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::~SubscribeHandler()8
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >&&)4
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()84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()10105
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()84
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&)84
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()168
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)84
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()1061
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::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() const2130
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() const11570
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() const98610
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() const260937
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]() const50
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() const78
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const84
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() const48007
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() const95215
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() const141294
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() const19566
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() const2
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() 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> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() 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> > >::hasMsg() const199
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]() const43
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() const41
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() const29689
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() const4472
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() const39
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const606
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() const4
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() const71348
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]() const49
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const39
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const606
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::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() const83
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() const65317
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const75041
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() const94555
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]() const3
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const10105
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const14974
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()() const42
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()() const42
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()() const40
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()() const37
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()() const42
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()() const42
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()() const45
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()() const27
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()() const37
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()() const39
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()() const39
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()() const2
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()() const39
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()() const91
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()() const42
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()() const39
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()() const39
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()() const51
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()() const42
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()() const9
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()() const39
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()() const42
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()() const155
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()() const126
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()() const42
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()() const42
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()() const39
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()() const42
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()() const42
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()() const0
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()() const42
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()() const126
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()() const42
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()() const42
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()() const42
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()() const252
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()() const0
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()() const18
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()() const51
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const9
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()() const9
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()() const42
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()() const210
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()() const51
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()() const2
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()() const2
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const9
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()() const42
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()() const42
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()() const45
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()() const2
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()() const42
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()() const42
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::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const4
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()() const84
+
+
+ + + +
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..2ea9974bd1 --- /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:394979.6 %
Date:2023-12-06 07:59:45Functions:35569950.8 %
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        1063 :     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  1473308486 :       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      732524 :       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      141294 :       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      170798 :       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         145 :       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        2614 :       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        3888 :       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        2610 :       SubscribeHandler(
+     199             :             const SubscribeHandlerOptions& options,
+     200             :             const std::string& topic_name,
+     201             :             Types ... args
+     202             :           )
+     203             :       :
+     204             :         SubscribeHandler(
+     205        2610 :             [options, topic_name]()
+     206             :             {
+     207        5220 :               SubscribeHandlerOptions opts = options;
+     208        2610 :               opts.topic_name = topic_name;
+     209        5220 :               return opts;
+     210             :             }(),
+     211             :             args...
+     212        2614 :             )
+     213             :       {
+     214        2610 :       }
+     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        2610 :       SubscribeHandler(
+     224             :             const SubscribeHandlerOptions& options,
+     225             :             const message_callback_t& message_callback = {}
+     226             :           )
+     227        2610 :       {
+     228        2610 :         if (options.threadsafe)
+     229             :         {
+     230        2610 :           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        2610 :         if (options.autostart)
+     245        2609 :           start();
+     246        2610 :       };
+     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        1042 :       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        2084 :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     324             :             args...
+     325        1042 :             )
+     326             :       {
+     327        1042 :       }
+     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        6496 :       ~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           0 :       SubscribeHandler(SubscribeHandler&& other)
+     419           0 :       {
+     420           0 :         this->m_pimpl = std::move(other.m_pimpl);
+     421           0 :         other.m_pimpl = nullptr;
+     422           0 :       }
+     423        2568 :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     424             :       {
+     425        2568 :         this->m_pimpl = std::move(other.m_pimpl);
+     426        2568 :         other.m_pimpl = nullptr;
+     427        2568 :         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..4fbffc4bc0b89a8b74c7423122e9f29b5abf23f7 GIT binary patch literal 1794 zcmV+d2mSboP)GAf72gj3K5r zbz2ND5}}4FQEmJ-0qHg3YyO-C@r%I16(bOAcI{2~uIrUq&|?~*Fbke#KB6~6I8Vnr z*#`C}y~&UtGp8aqYx(}~$v#>;vYCZ!J|t)Of*?k(LiRez2b~a-7TcQx@sJ(OR zSq>>wvJ`~KR`Ok-ZVp=~gM8B6WXF=d4O(*_&BL&Z@Q&A3jZ={@1qCG!No<}!R9n~W zb*{9j_GML&O^1ys1CB351|2_`Jt2(&lZCYg(hT|2F^wA_6OB9MF{bcvYuq&&R7fFo zsE;bOL1Kh32wTn^>m!tz668ytM&>Z-6K5u6q!$AOc%jcH>3fS({;hPmwMn+C2MvDs~C)tCPOw1-3bupJYJgBin5(kdU98OtK zN-*pu2VTAz1vBYHiE0uiLlN>kL#(mA6FXs73unw;3sz_yc$`3{Fzh2wZJ5Y%O{q?O z6g)UZIm3|l&g)}FbThr1oSPcWrg26f^YA!E?wTS*9S^FU0fyKGkEa3RWj`QnTtj@P)_8$ z2uGra;0+aAu({!p*B^pM3f2OmBou>R#tqPAJ6&N4mRxM#Slrw2_gpYNmF=GG1RVEwfr_%69B zg1?$lN=5+@BubI(e|^i#a1Wu_TKJigkY$BZh;SQ%asVCl1<+MtJ#PXHO}{QliD=<% z>z+eDCe1n-oV|-Sf6kQh2wK~lq+XMx=F1AC21)TJ)y+ReQUejf z5dmCx>)nM`Fd{f87~WNiSz{ifA0U9gK`C-_jAmQh!l`dlDolYY8-l7ng5m-SbGmz7 z44N9nFSo{o#1MJo;|=zwAUudO%yjXuR8o)D(idg)oLq95htT!Ji>bW~y~$N6c$RxX$#k;(w-iXq@Ae&+a*xjc`l|iCimSGlp%iNo zmj%`H0P#WN3-4Y}mr+V7)im&1D8-{wa=#<^7t(cByX);opz)k*;UC3quL~auEb4Dl zT8W&D=)Uv(kGprIXeCG~98fOQqBoytmhDzK)|^ZSnsoSydTxc*kzh}w&9oYs-BI^a zEccy<@w;P`+MdpX;7H>#+JCV3(b3E~J&o67TF}BN65#E%=;WdLvuEMMj|@=G@{dDt z7s6i(_s=}DAT6RH?&c*ky_O0Ocuh4s@m0V-oOQ8qpvR6G9r_vpQ-Xn1^1rHIdE#T$ zNdSM|*$f}QGq@uJE&QmM;VK4iP#ZP~q}8+@>&_WhvDa%+3KqE^Ej{*I0#9i?y$G-T kuY(Tel-8&otZ%#hAJepWR?y0d2mk;807*qoM6N<$f{w&dzW@LL 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..8f13844fb4 --- /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:4666.7 %
Date:2023-12-06 07:59:45Functions: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..db3f677d26 --- /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:4666.7 %
Date:2023-12-06 07:59:45Functions: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..b0bff7641e --- /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:4666.7 %
Date:2023-12-06 07:59:45Functions: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           0 :   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          11 :       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       13919 :       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          15 :       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           0 :         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..fb548ce0cdbff8a4352875b509c0e585c652d55b GIT binary patch literal 466 zcmV;@0WJQCP)iw^>LI6YShQZq1^!c8(!!Y@fRL7o~IGHnz$@d_| zgc1X?T$DAO>hOf^iyK#`_nTXQBU>k*(O_bXgy_Q^$Ob(a+0JO0D^=Ljo-R$ z&rNK#%&kxJ_X8+p0jl)=sB8%W>nD|_7Ft?hF=n79E^9B9=tXC8<4^~ehPTHB|6l#M z#^oGss#!pKXJgYeEy?C(X>LNkm#=dKLMDu}q6(2Kta9o+c#myZ}jkGxg<0Od-EP$7d#-T(jq07*qo IM6N<$f@@{U^8f$< literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/timer.h.func-sort-c.html b/mrs_lib/include/mrs_lib/timer.h.func-sort-c.html new file mode 100644 index 0000000000..67983c2d87 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.func-sort-c.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2023-12-06 07:59:45Functions: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..471c6239ff --- /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:2023-12-06 07:59:45Functions: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..bd11a237b0 --- /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:2023-12-06 07:59:45Functions: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:2023-12-06 07:59:45Functions: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::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)138
mrs_lib::Transformer::retryLookupNewest(bool)169
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)121622
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)687107
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)687118
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)690217
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&)690220
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)690703
+
+
+ + + +
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..99a941f453 --- /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:2023-12-06 07:59:45Functions: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&)687107
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)690217
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)121622
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&)690220
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)138
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)169
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&)687118
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)690703
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..e6826bdc34 --- /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:2023-12-06 07:59:45Functions: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      121622 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132      211300 :       std::scoped_lock lck(mutex_);
+     133      121613 :       default_frame_id_ = frame_id;
+     134      121616 :     }
+     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         138 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155         234 :       std::scoped_lock lck(mutex_);
+     156         138 :       if (prefix.empty())
+     157          43 :         prefix_ = "";
+     158             :       else
+     159         137 :         prefix_ = prefix + "/";
+     160         138 :     }
+     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         211 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213         253 :       std::scoped_lock lck(mutex_);
+     214         211 :       retry_lookup_newest_ = retry;
+     215         169 :     }
+     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           6 :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250          10 :       std::scoped_lock lck(mutex_);
+     251          12 :       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      687107 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552      687107 :       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      690217 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565      690217 :       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      687118 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576      687118 :       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      690703 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589      690703 :       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      690220 :     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      690220 :       geometry_msgs::TransformStamped ret;
+     621      690181 :       frame_from(ret) = from_frame;
+     622      690234 :       frame_to(ret) = to_frame;
+     623      690203 :       ret.header.stamp = time_stamp;
+     624      690203 :       ret.transform = tf;
+     625      690203 :       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:2023-12-06 07:59:45Functions: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..d445cf70f7 --- /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:2023-12-06 07:59:45Functions: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..7ee01fab0f --- /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:2023-12-06 07:59:45Functions: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..c4a1383c48 --- /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:2023-12-06 07:59:45Functions: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)19276
+
+
+ + + +
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..dc3e8ae44b --- /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:2023-12-06 07:59:45Functions: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)19276
+
+
+ + + +
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..21f8fee5db --- /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:2023-12-06 07:59:45Functions: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       19276 :   int signum(T val)
+     139             :   {
+     140       19276 :     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/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:2023-12-06 07:59:45Functions: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&) const6
+
+
+ + + +
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..ec8ee4c6e5 --- /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:2023-12-06 07:59:45Functions: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&) const6
+
+
+ + + +
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..ad432d9e72 --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.gcov.html @@ -0,0 +1,182 @@ + + + + + + + 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:2023-12-06 07:59:45Functions: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/TrajectoryReference.h>
+      15             : 
+      16             : #define DEFAULT_ELLIPSE_POINTS 64
+      17             : 
+      18             : namespace mrs_lib
+      19             : {
+      20             : 
+      21             : enum MarkerType
+      22             : {
+      23             :   POINT    = 0,
+      24             :   LINE     = 1,
+      25             :   TRIANGLE = 2
+      26             : };
+      27             : 
+      28             : class VisualObject {
+      29             : 
+      30             : 
+      31             : public:
+      32             :   VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      33             :                const unsigned long& id);
+      34             : 
+      35             :   VisualObject(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      36             :                const unsigned long& id);
+      37             : 
+      38             :   VisualObject(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      39             :                const bool filled, const unsigned long& id);
+      40             : 
+      41             :   VisualObject(const mrs_lib::geometry::Rectangle& rectangle, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      42             :                const bool filled, const unsigned long& id);
+      43             : 
+      44             :   VisualObject(const mrs_lib::geometry::Cuboid& cuboid, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      45             :                const bool filled, const unsigned long& id);
+      46             : 
+      47             :   VisualObject(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      48             :                const bool filled, const unsigned long& id, const int num_points = DEFAULT_ELLIPSE_POINTS);
+      49             : 
+      50             :   VisualObject(const mrs_lib::geometry::Cylinder& cylinder, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      51             :                const bool filled, const bool capped, const unsigned long& id, const int num_sides = DEFAULT_ELLIPSE_POINTS);
+      52             : 
+      53             :   VisualObject(const mrs_lib::geometry::Cone& cone, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      54             :                const bool filled, const bool capped, const unsigned long& id, const int num_sides = DEFAULT_ELLIPSE_POINTS);
+      55             : 
+      56             :   VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+      57             :                const bool filled, const unsigned long& id);
+      58             : 
+      59             : 
+      60             : public:
+      61             :   unsigned long getID() const;
+      62             :   int           getType() const;
+      63             :   bool          isTimedOut() const;
+      64             : 
+      65             :   const std::vector<geometry_msgs::Point> getPoints() const;
+      66             :   const std::vector<std_msgs::ColorRGBA>  getColors() const;
+      67             : 
+      68           6 :   bool operator<(const VisualObject& other) const {
+      69           6 :     return id_ < other.id_;
+      70             :   }
+      71             : 
+      72             :   bool operator>(const VisualObject& other) const {
+      73             :     return id_ > other.id_;
+      74             :   }
+      75             : 
+      76             :   bool operator==(const VisualObject& other) const {
+      77             :     return id_ == other.id_;
+      78             :   }
+      79             : 
+      80             : private:
+      81             :   const unsigned long               id_;
+      82             :   MarkerType                        type_;
+      83             :   std::vector<geometry_msgs::Point> points_;
+      84             :   std::vector<std_msgs::ColorRGBA>  colors_;
+      85             :   ros::Time                         timeout_time_;
+      86             : 
+      87             :   void addRay(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a);
+      88             : 
+      89             :   void addTriangle(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a, const bool filled);
+      90             : 
+      91             :   void 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             : };  // namespace batch_visualizer
+      95             : 
+      96             : }  // namespace mrs_lib
+      97             : 
+      98             : #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..a2ea444b1d --- /dev/null +++ b/mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html @@ -0,0 +1,45 @@ + + + + + + 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 + + +
+ 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..e5a217429afe8a62a7258891b3370af79723c76a GIT binary patch literal 484 zcmV_%GNWv0>g zGt_r9t5M~ynG$$UJ_W8188j)if_*u)woji{4E;^-{UY~_0nSSu_-j7Nl$zcH=Mcti z)LgHKKcR-lN5XuYPz`FHO2WO58WLXINgWlGxZkP6ZEO*qtF;Z}`~XH;uSyM>+#9J& z_6GG-tp`#ZSfnJqHx)P_c9HB8iB{~xc}6NHdtoC-LT$~lM;Ie{9)mn+hs+wZj;yw~ z2H4EHsB!qhXOsUFX`cCk=0ilxm(p|t`4kb#faQ^kN(r;d_&|oJg?}!bGq6F1RhxzO aNB#jj9b7t&#$cTQ0000 + + + + + + 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:2023-12-06 07:59:45Functions: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&)7150
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)33442
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()43306
mrs_lib::AttitudeConverter::getYaw()58837
mrs_lib::AttitudeConverter::calculateRPY()58842
mrs_lib::AttitudeConverter::setHeading(double const&)93098
mrs_lib::AttitudeConverter::getVectorZ()95101
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const96389
mrs_lib::AttitudeConverter::operator tf2::Transform() const124076
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)125441
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)132591
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)145594
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const227698
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const329253
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)338569
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const624498
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)648320
mrs_lib::AttitudeConverter::getHeading()674171
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)1333005
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)2160432
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const2764736
mrs_lib::AttitudeConverter::validateOrientation()4625467
+
+
+ + + +
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..5fc1d786ff --- /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:2023-12-06 07:59:45Functions: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&)125441
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)7150
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()674171
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()95101
mrs_lib::AttitudeConverter::setHeading(double const&)93098
mrs_lib::AttitudeConverter::calculateRPY()58842
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)132591
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)33442
mrs_lib::AttitudeConverter::validateOrientation()4625467
mrs_lib::AttitudeConverter::getYaw()58837
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> >)1333005
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)338569
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)145594
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)648320
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&)2160432
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()43306
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>() const227698
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const2764736
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const329253
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const96389
mrs_lib::AttitudeConverter::operator tf2::Transform() const124076
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const624498
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..5a73aadff1 --- /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:2023-12-06 07:59:45Functions: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        7150 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34        7150 :   vector3_[0] = vector3[0];
+      35        7150 :   vector3_[1] = vector3[1];
+      36        7150 :   vector3_[2] = vector3[2];
+      37        7150 : }
+      38             : 
+      39      125441 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41      125442 :   vector3_[0] = vector3.x;
+      42      125441 :   vector3_[1] = vector3.y;
+      43      125442 :   vector3_[2] = vector3.z;
+      44      125442 : }
+      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      227698 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60      227698 :   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     2160432 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80     2160214 :   switch (format) {
+      81             : 
+      82     2160245 :     case RPY_EXTRINSIC: {
+      83     2160245 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84     2160404 :       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     2160373 :   validateOrientation();
+     104     2160366 : }
+     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     1333005 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117     1332959 :   tf2_quaternion_.setX(quaternion.x);
+     118     1332842 :   tf2_quaternion_.setY(quaternion.y);
+     119     1332967 :   tf2_quaternion_.setZ(quaternion.z);
+     120     1333013 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122     1333035 :   validateOrientation();
+     123     1332751 : }
+     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      145594 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132      145594 :   tf2_quaternion_.setX(quaternion.x());
+     133      145594 :   tf2_quaternion_.setY(quaternion.y());
+     134      145594 :   tf2_quaternion_.setZ(quaternion.z());
+     135      145594 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137      145594 :   validateOrientation();
+     138      145593 : }
+     139             : 
+     140      648320 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141      647815 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143      648085 :   tf2_quaternion_.setX(quaternion.x());
+     144      647845 :   tf2_quaternion_.setY(quaternion.y());
+     145      648021 :   tf2_quaternion_.setZ(quaternion.z());
+     146      648005 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148      648010 :   validateOrientation();
+     149      647519 : }
+     150             : 
+     151      338569 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153      337815 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155      337815 :   validateOrientation();
+     156      337822 : }
+     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      329253 : AttitudeConverter::operator tf2::Quaternion() const {
+     170      329253 :   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     2764736 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187     2764736 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189     2763985 :   geom_quaternion.x = tf2_quaternion_.x();
+     190     2763216 :   geom_quaternion.y = tf2_quaternion_.y();
+     191     2763264 :   geom_quaternion.z = tf2_quaternion_.z();
+     192     2763545 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194     2763574 :   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      624498 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207      624498 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209     1248871 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212       43306 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214       43306 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216       43306 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219       96389 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221       96389 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224      124076 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226      124076 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233       58837 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235       58837 :   calculateRPY();
+     236             : 
+     237       58837 :   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      674171 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256      674171 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258      674058 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260      673841 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           1 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264     1346832 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267       33442 : 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       33442 :   if (fabs(heading_rate) < 1e-3) {
+     272       19848 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276       13594 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279       13594 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280       13594 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283       13594 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284       13594 :   b_orb.normalize();
+     285       13594 :   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       13594 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291       13594 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292       13594 :   double projected_norm        = projected.norm();
+     293             : 
+     294       13594 :   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       13594 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301       13594 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303       13594 :   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       13594 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312      132591 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315      132591 :   Eigen::Matrix3d R = *this;
+     316      132590 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319      132591 :   Eigen::Matrix3d W;
+     320      132591 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323      132589 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326      132591 :   double rx = R(0, 0);  // x-component of body X
+     327      132589 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329      132589 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331      132589 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336      132589 :   double atan2_d_x = -ry / denom;
+     337      132589 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340      132589 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342      132591 :   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       95101 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365       95101 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367       95101 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369      190202 :   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       93098 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406       93098 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409       93098 :   if (fabs(b3[2]) < 1e-3) {
+     410           2 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414       93096 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416       93096 :   Eigen::Matrix3d new_R;
+     417             : 
+     418       93095 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421       93095 :   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      185961 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425       93051 :   A.col(0)          = projector_body_z_compl.col(0);
+     426       93036 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429      185856 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430       93055 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431       93036 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434      185802 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435      185886 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436       92935 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438       92896 :   new_R.col(0) = oblique_projector * h;
+     439       92863 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443       92836 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444       92452 :   new_R.col(1).normalize();
+     445             : 
+     446      185664 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455       58842 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457       58842 :   if (!got_rpy_) {
+     458             : 
+     459       58842 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461       58842 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467     4625467 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469     9249177 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470     4623091 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473     4623399 : }
+     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..9e90b8f899 --- /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:2023-12-06 07:59:45Functions: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..a1c60e0205 --- /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:2023-12-06 07:59:45Functions: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..52e779c776 --- /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:2023-12-06 07:59:45Functions: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..021de1cfce --- /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:2023-12-06 07:59:45Functions: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..6459455994 --- /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:2023-12-06 07:59:45Functions: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..505bfedcf8 --- /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:2023-12-06 07:59:45Functions: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..8a1c9f71a3 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + 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:16221376.1 %
Date:2023-12-06 07:59:45Functions:132259.1 %
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::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> >)2
mrs_lib::BatchVisualizer::setPointsScale(double)2
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)4
mrs_lib::BatchVisualizer::initialize()84
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> >)84
mrs_lib::BatchVisualizer::BatchVisualizer()84
mrs_lib::BatchVisualizer::clearBuffers()86
mrs_lib::BatchVisualizer::clearVisuals()86
mrs_lib::BatchVisualizer::~BatchVisualizer()168
mrs_lib::BatchVisualizer::addNullPoint()170
mrs_lib::BatchVisualizer::addNullLine()172
mrs_lib::BatchVisualizer::addNullTriangle()172
mrs_lib::BatchVisualizer::publish()172
+
+
+ + + +
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..e7887a6e3d --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + 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:16221376.1 %
Date:2023-12-06 07:59:45Functions:132259.1 %
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()84
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullLine()172
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullPoint()170
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::clearBuffers()86
mrs_lib::BatchVisualizer::clearVisuals()86
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> >)2
mrs_lib::BatchVisualizer::setPointsScale(double)2
mrs_lib::BatchVisualizer::addNullTriangle()172
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::publish()172
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)4
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> >)84
mrs_lib::BatchVisualizer::BatchVisualizer()84
mrs_lib::BatchVisualizer::~BatchVisualizer()168
+
+
+ + + +
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..e38347c84c --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html @@ -0,0 +1,432 @@ + + + + + + + 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:16221376.1 %
Date:2023-12-06 07:59:45Functions:132259.1 %
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          84 : BatchVisualizer::BatchVisualizer() {
+      10          84 : }
+      11             : 
+      12         168 : BatchVisualizer::~BatchVisualizer() {
+      13         168 : }
+      14             : 
+      15          84 : BatchVisualizer::BatchVisualizer(ros::NodeHandle &nh, const std::string marker_topic_name, const std::string parent_frame) {
+      16          84 :   this->parent_frame      = parent_frame;
+      17          84 :   this->marker_topic_name = marker_topic_name;
+      18          84 :   initialize();
+      19             : 
+      20          84 :   this->visual_pub = nh.advertise<visualization_msgs::MarkerArray>(marker_topic_name.c_str(), 1);
+      21          84 :   publish();
+      22          84 : }
+      23             : //}
+      24             : 
+      25             : /* setParentFrame //{ */
+      26             : 
+      27           2 : void BatchVisualizer::setParentFrame(const std::string parent_frame) {
+      28           2 :   this->parent_frame               = parent_frame;
+      29           2 :   points_marker.header.frame_id    = parent_frame;
+      30           2 :   triangles_marker.header.frame_id = parent_frame;
+      31           2 :   lines_marker.header.frame_id     = parent_frame;
+      32           2 : }
+      33             : 
+      34             : //}
+      35             : 
+      36             : /* initialize //{ */
+      37          84 : void BatchVisualizer::initialize() {
+      38          84 :   if (initialized) {
+      39           0 :     return;
+      40             :   }
+      41             : 
+      42             :   // setup points marker
+      43          84 :   std::stringstream ss;
+      44          84 :   ss << marker_topic_name << "_points";
+      45          84 :   points_marker.header.frame_id    = parent_frame;
+      46          84 :   points_marker.header.stamp       = ros::Time::now();
+      47          84 :   points_marker.ns                 = ss.str().c_str();
+      48          84 :   points_marker.action             = visualization_msgs::Marker::ADD;
+      49          84 :   points_marker.pose.orientation.w = 1.0;
+      50          84 :   points_marker.id                 = 8;
+      51          84 :   points_marker.type               = visualization_msgs::Marker::POINTS;
+      52             : 
+      53          84 :   points_marker.scale.x = points_scale;
+      54          84 :   points_marker.scale.y = points_scale;
+      55          84 :   points_marker.color.a = 1.0;
+      56             : 
+      57             :   // setup lines marker
+      58          84 :   ss.str(std::string());
+      59          84 :   ss << marker_topic_name << "_lines";
+      60          84 :   lines_marker.header.frame_id    = parent_frame;
+      61          84 :   lines_marker.header.stamp       = ros::Time::now();
+      62          84 :   lines_marker.ns                 = ss.str().c_str();
+      63          84 :   lines_marker.action             = visualization_msgs::Marker::ADD;
+      64          84 :   lines_marker.pose.orientation.w = 1.0;
+      65          84 :   lines_marker.id                 = 5;
+      66          84 :   lines_marker.type               = visualization_msgs::Marker::LINE_LIST;
+      67             : 
+      68          84 :   lines_marker.scale.x = lines_scale;
+      69          84 :   lines_marker.color.a = 1.0;
+      70             : 
+      71             :   // setup triangles marker
+      72          84 :   ss.str(std::string());
+      73          84 :   ss << marker_topic_name << "_triangles";
+      74          84 :   triangles_marker.header.frame_id    = parent_frame;
+      75          84 :   triangles_marker.header.stamp       = ros::Time::now();
+      76          84 :   triangles_marker.ns                 = ss.str().c_str();
+      77          84 :   triangles_marker.action             = visualization_msgs::Marker::ADD;
+      78          84 :   triangles_marker.pose.orientation.w = 1.0;
+      79          84 :   triangles_marker.id                 = 11;
+      80          84 :   triangles_marker.type               = visualization_msgs::Marker::TRIANGLE_LIST;
+      81             : 
+      82          84 :   triangles_marker.scale.x = 1;
+      83          84 :   triangles_marker.scale.y = 1;
+      84          84 :   triangles_marker.scale.z = 1;
+      85          84 :   triangles_marker.color.a = 1.0;
+      86             : 
+      87          84 :   ROS_INFO("[%s]: Batch visualizer loaded with default values", ros::this_node::getName().c_str());
+      88          84 :   initialized = true;
+      89             : }
+      90             : //}
+      91             : 
+      92             : /* addPoint //{ */
+      93           4 : 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           8 :   VisualObject obj = VisualObject(p, r, g, b, a, timeout, uuid++);
+      96           4 :   visual_objects.insert(obj);
+      97           4 : }
+      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             : /* addTrajectory //{ */
+     161           0 : void BatchVisualizer::addTrajectory(const mrs_msgs::TrajectoryReference &traj, const double r, const double g, const double b, const double a,
+     162             :                                     const bool filled, const ros::Duration &timeout) {
+     163           0 :   VisualObject obj = VisualObject(traj, r, g, b, a, timeout, filled, uuid++);
+     164           0 :   visual_objects.insert(obj);
+     165           0 : }
+     166             : //}
+     167             : 
+     168             : /* addNullPoint //{ */
+     169         170 : void BatchVisualizer::addNullPoint() {
+     170         170 :   geometry_msgs::Point p;
+     171         170 :   p.x = 10000.0;
+     172         170 :   p.y = 0.0;
+     173         170 :   p.z = 0.0;
+     174             : 
+     175         170 :   std_msgs::ColorRGBA c;
+     176         170 :   c.r = 1.0;
+     177         170 :   c.g = 1.0;
+     178         170 :   c.b = 1.0;
+     179         170 :   c.a = 1.0;
+     180             : 
+     181         170 :   points_marker.points.push_back(p);
+     182         170 :   points_marker.colors.push_back(c);
+     183         170 : }
+     184             : //}
+     185             : 
+     186             : /* addNullLine //{ */
+     187         172 : void BatchVisualizer::addNullLine() {
+     188         172 :   geometry_msgs::Point p1, p2;
+     189         172 :   p1.x = 10000.0;
+     190         172 :   p1.y = 0.0;
+     191         172 :   p1.z = 0.0;
+     192             : 
+     193         172 :   p2.x = 10001.0;
+     194         172 :   p2.y = 0.0;
+     195         172 :   p2.z = 0.0;
+     196             : 
+     197         172 :   std_msgs::ColorRGBA c;
+     198         172 :   c.r = 1.0;
+     199         172 :   c.g = 1.0;
+     200         172 :   c.b = 1.0;
+     201             : 
+     202         172 :   lines_marker.colors.push_back(c);
+     203         172 :   lines_marker.colors.push_back(c);
+     204             : 
+     205         172 :   lines_marker.points.push_back(p1);
+     206         172 :   lines_marker.points.push_back(p2);
+     207         172 : }
+     208             : //}
+     209             : 
+     210             : /* addNullTriangle //{ */
+     211         172 : void BatchVisualizer::addNullTriangle() {
+     212         172 :   geometry_msgs::Point p1, p2, p3;
+     213         172 :   p1.x = 10000.0;
+     214         172 :   p1.y = 0.0;
+     215         172 :   p1.z = 0.0;
+     216             : 
+     217         172 :   p2.x = 10001.0;
+     218         172 :   p2.y = 0.0;
+     219         172 :   p2.z = 0.0;
+     220             : 
+     221         172 :   std_msgs::ColorRGBA c;
+     222         172 :   c.r = 1.0;
+     223         172 :   c.g = 1.0;
+     224         172 :   c.b = 1.0;
+     225             : 
+     226         172 :   p3.x = 10001.0;
+     227         172 :   p3.y = 0.01;
+     228         172 :   p3.z = 0.0;
+     229         172 :   triangles_marker.colors.push_back(c);
+     230         172 :   triangles_marker.colors.push_back(c);
+     231         172 :   triangles_marker.colors.push_back(c);
+     232             : 
+     233         172 :   triangles_marker.points.push_back(p1);
+     234         172 :   triangles_marker.points.push_back(p2);
+     235         172 :   triangles_marker.points.push_back(p3);
+     236         172 : }
+     237             : //}
+     238             : 
+     239             : /* setPointsScale //{ */
+     240           2 : void BatchVisualizer::setPointsScale(const double scale) {
+     241           2 :   points_scale = scale;
+     242           2 : }
+     243             : //}
+     244             : 
+     245             : /* setLinesScale //{ */
+     246           0 : void BatchVisualizer::setLinesScale(const double scale) {
+     247           0 :   lines_scale = scale;
+     248           0 : }
+     249             : //}
+     250             : 
+     251             : /* clearBuffers //{ */
+     252          86 : void BatchVisualizer::clearBuffers() {
+     253          86 :   visual_objects.clear();
+     254          86 : }
+     255             : //}
+     256             : 
+     257             : /* clearVisuals //{ */
+     258          86 : void BatchVisualizer::clearVisuals() {
+     259         172 :   std::set<VisualObject> visual_objects_tmp;
+     260          86 :   visual_objects_tmp.insert(visual_objects.begin(), visual_objects.end());
+     261             : 
+     262          86 :   visual_objects.clear();
+     263          86 :   publish();
+     264             : 
+     265          86 :   visual_objects.insert(visual_objects_tmp.begin(), visual_objects_tmp.end());
+     266          86 : }
+     267             : //}
+     268             : 
+     269             : /* publish //{ */
+     270         172 : void BatchVisualizer::publish() {
+     271             : 
+     272         172 :   msg.markers.clear();
+     273         172 :   points_marker.points.clear();
+     274         172 :   points_marker.colors.clear();
+     275             : 
+     276         172 :   lines_marker.points.clear();
+     277         172 :   lines_marker.colors.clear();
+     278             : 
+     279         172 :   triangles_marker.points.clear();
+     280         172 :   triangles_marker.colors.clear();
+     281             : 
+     282             :   // fill marker messages and remove objects that have timed out
+     283         176 :   for (auto it = visual_objects.begin(); it != visual_objects.end();) {
+     284           4 :     if (it->isTimedOut()) {
+     285           0 :       it = visual_objects.erase(it);
+     286             :     } else {
+     287           8 :       auto points = it->getPoints();
+     288           8 :       auto colors = it->getColors();
+     289           4 :       switch (it->getType()) {
+     290           4 :         case MarkerType::POINT: {
+     291           4 :           points_marker.points.insert(points_marker.points.end(), points.begin(), points.end());
+     292           4 :           points_marker.colors.insert(points_marker.colors.end(), colors.begin(), colors.end());
+     293           4 :           break;
+     294             :         }
+     295           0 :         case MarkerType::LINE: {
+     296           0 :           lines_marker.points.insert(lines_marker.points.end(), points.begin(), points.end());
+     297           0 :           lines_marker.colors.insert(lines_marker.colors.end(), colors.begin(), colors.end());
+     298           0 :           break;
+     299             :         }
+     300           0 :         case MarkerType::TRIANGLE: {
+     301           0 :           triangles_marker.points.insert(triangles_marker.points.end(), points.begin(), points.end());
+     302           0 :           triangles_marker.colors.insert(triangles_marker.colors.end(), colors.begin(), colors.end());
+     303           0 :           break;
+     304             :         }
+     305             :       }
+     306           4 :       it++;
+     307             :     }
+     308             :   }
+     309             : 
+     310         172 :   auto now = ros::Time::now();
+     311             : 
+     312         172 :   if (!points_marker.points.empty()) {
+     313           2 :     points_marker.scale.x = points_scale;
+     314           2 :     points_marker.scale.y = points_scale;
+     315             :   } else {
+     316         170 :     addNullPoint();
+     317             :   }
+     318         172 :   points_marker.header.stamp = now;
+     319         172 :   msg.markers.push_back(points_marker);
+     320             : 
+     321         172 :   if (!lines_marker.points.empty()) {
+     322           0 :     lines_marker.scale.x = lines_scale;
+     323             :   } else {
+     324         172 :     addNullLine();
+     325             :   }
+     326         172 :   lines_marker.header.stamp = now;
+     327         172 :   msg.markers.push_back(lines_marker);
+     328             : 
+     329         172 :   if (!triangles_marker.points.empty()) {
+     330           0 :     triangles_marker.header.stamp = now;
+     331             :   } else {
+     332         172 :     addNullTriangle();
+     333             :   }
+     334         172 :   triangles_marker.header.stamp = now;
+     335         172 :   msg.markers.push_back(triangles_marker);
+     336             : 
+     337         172 :   if (msg.markers.empty()) {
+     338           0 :     addNullPoint();
+     339           0 :     points_marker.scale.x = 0.1;
+     340           0 :     points_marker.scale.y = 0.1;
+     341           0 :     msg.markers.push_back(points_marker);
+     342             :   }
+     343             : 
+     344         172 :   visual_pub.publish(msg);
+     345         172 : }
+     346             : //}
+     347             : 
+     348             : }  // 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..3e4e83238a --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html @@ -0,0 +1,107 @@ + + + + + + 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 + + +
+ 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..83593fe269cd764b2c7f8a49c73950ce03af9ca5 GIT binary patch literal 1246 zcmV<41R?v0P)4FDM+LWy#*X4g@_z#U54m6z$Bm9mkbWCO(C2m zybIVcEp20v*^5gna>5y}fy^ji#Vz4_2{cF7{0jn%yE$Mykgl+Wi$G=+>zHZA>oo#l zQG+t`k|pw<5=aM&e)X{W`9wYF88844`hn4mkOdS44{YlPwvo^eZW`&JA^_BF%*&8^ zj4Vi1+z%ub+3!rux)=Llai89iDi1Wh(awP63DQk@k0ft}8R-(Yi|4>>yLnvF6yO|{ z%bG~639rQa0gv}g0r-jQS&m~rijsZR;dShKlI40PEe!SSv8r9$m#gwPN?Cp$yBcj~r@F)lrkVjp8cB%x|!ji>3W*;bz2#foJc$83yUE(zS zis-ST(vv6U@ljVUK_o@y3e72i%l%tEBM>cZb z6&>02xF$#TQ^w2_+W4o&br0dZ?XI2?#fm$C!h+S+F`25wWRI`)5ps#Q8I-SA*i;~~jRHH$PnY&WarUs6f zCY?D={(4_a7O$$lCMHsY^oW7YUhDu4w6WRF4&b0^vd#{m)U;8>93Fy>pX@&Sj7JoO za(CB@r=ylNSBlfgX*%sBrDK3+crF%&sf)u2)af2oE7+g4y36j`=BE>w?yJ+zs8A*2 zxa*m^8*?T5Z9kBvIx{VaDt0k7ySS6oyH!_^1AvuU-4A5@+Pq1UfZ7?-9j=YBo&CV; zoSCFg`CCZ{I(w2y!bFP8pL9{hqbAn`QfZbVeD<;fxU$a`S2Z%cPr{C=uL|^}^Okv` zaMd**Fh1y`f}e!3eoH*EZ5-mUvA}@wNC1Te5iYUHRREOvd4i(qZK#&xV!Z literal 0 HcmV?d00001 diff --git a/mrs_lib/src/batch_visualizer/index-detail-sort-f.html b/mrs_lib/src/batch_visualizer/index-detail-sort-f.html new file mode 100644 index 0000000000..e148c9d6bd --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail-sort-f.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:19240847.1 %
Date:2023-12-06 07:59:45Functions:204346.5 %
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 +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
<unnamed>15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
<unnamed>76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
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..bca02eed08 --- /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:19240847.1 %
Date:2023-12-06 07:59:45Functions:204346.5 %
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 +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
<unnamed>15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
<unnamed>76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
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..ec753663a9 --- /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:19240847.1 %
Date:2023-12-06 07:59:45Functions:204346.5 %
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 +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
<unnamed>76.1 %162 / 21359.1 %13 / 22
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
<unnamed>15.4 %30 / 19533.3 %7 / 21
+
+
+ + + + +
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..54439d5787 --- /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:19240847.1 %
Date:2023-12-06 07:59:45Functions:204346.5 %
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 +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
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..794be5dfc2 --- /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:19240847.1 %
Date:2023-12-06 07:59:45Functions:204346.5 %
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 +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
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..c2a83cf0f0 --- /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:19240847.1 %
Date:2023-12-06 07:59:45Functions:204346.5 %
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 +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
+
+
+ + + + +
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..a1ea7f737b --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html @@ -0,0 +1,164 @@ + + + + + + + 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:3019515.4 %
Date:2023-12-06 07:59:45Functions:72133.3 %
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_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::eigenToMsg(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)4
mrs_lib::VisualObject::VisualObject(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&, unsigned long const&)4
mrs_lib::generateColor(double, double, double, double)4
mrs_lib::VisualObject::isTimedOut() const4
mrs_lib::VisualObject::getType() const4
mrs_lib::VisualObject::getColors() const4
mrs_lib::VisualObject::getPoints() const4
+
+
+ + + +
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..4dd54dc98a --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func.html @@ -0,0 +1,164 @@ + + + + + + + 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:3019515.4 %
Date:2023-12-06 07:59:45Functions:72133.3 %
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&)4
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&)4
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_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)4
mrs_lib::VisualObject::isTimedOut() const4
mrs_lib::VisualObject::getID() const0
mrs_lib::VisualObject::getType() const4
mrs_lib::VisualObject::getColors() const4
mrs_lib::VisualObject::getPoints() const4
+
+
+ + + +
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..0d8cc08d0a --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html @@ -0,0 +1,401 @@ + + + + + + + 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:3019515.4 %
Date:2023-12-06 07:59:45Functions:72133.3 %
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           4 : geometry_msgs::Point eigenToMsg(const Eigen::Vector3d& v) {
+      10           4 :   geometry_msgs::Point p;
+      11           4 :   p.x = v.x();
+      12           4 :   p.y = v.y();
+      13           4 :   p.z = v.z();
+      14           4 :   return p;
+      15             : }
+      16             : 
+      17           4 : std_msgs::ColorRGBA generateColor(const double r, const double g, const double b, const double a) {
+      18           4 :   std_msgs::ColorRGBA c;
+      19           4 :   c.r = r;
+      20           4 :   c.g = g;
+      21           4 :   c.b = b;
+      22           4 :   c.a = a;
+      23           4 :   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           4 : VisualObject::VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     116           4 :                            const unsigned long& id)
+     117           4 :     : id_(id) {
+     118           4 :   type_ = MarkerType::POINT;
+     119           4 :   points_.push_back(eigenToMsg(point));
+     120           4 :   colors_.push_back(generateColor(r, g, b, a));
+     121           4 :   if (timeout.toSec() <= 0) {
+     122           4 :     timeout_time_ = ros::Time(0);
+     123             :   } else {
+     124           0 :     timeout_time_ = ros::Time::now() + timeout;
+     125             :   }
+     126           4 : }
+     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::TrajectoryReference //{ */
+     254           0 : VisualObject::VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a,
+     255           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     256           0 :     : id_(id) {
+     257           0 :   if (traj.points.size() < 2) {
+     258           0 :     return;
+     259             :   }
+     260           0 :   if (filled) {
+     261           0 :     for (size_t i = 0; i < traj.points.size() - 1; i++) {
+     262           0 :       Eigen::Vector3d p1, p2;
+     263           0 :       p1.x()   = traj.points[i].position.x;
+     264           0 :       p1.y()   = traj.points[i].position.y;
+     265           0 :       p1.z()   = traj.points[i].position.z;
+     266           0 :       p2.x()   = traj.points[i + 1].position.x;
+     267           0 :       p2.y()   = traj.points[i + 1].position.y;
+     268           0 :       p2.z()   = traj.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 < traj.points.size(); i++) {
+     275           0 :       points_.push_back(traj.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             : /* getID //{ */
+     288           0 : unsigned long VisualObject::getID() const {
+     289           0 :   return id_;
+     290             : }
+     291             : //}
+     292             : 
+     293             : /* getType //{ */
+     294           4 : int VisualObject::getType() const {
+     295           4 :   return type_;
+     296             : }
+     297             : //}
+     298             : 
+     299             : /* isTimedOut //{ */
+     300           4 : bool VisualObject::isTimedOut() const {
+     301           4 :   return !timeout_time_.isZero() && (timeout_time_ - ros::Time::now()).toSec() <= 0;
+     302             : }
+     303             : //}
+     304             : 
+     305             : /* getPoints //{ */
+     306           4 : const std::vector<geometry_msgs::Point> VisualObject::getPoints() const {
+     307           4 :   return points_;
+     308             : }
+     309             : //}
+     310             : 
+     311             : /* getColors //{ */
+     312           4 : const std::vector<std_msgs::ColorRGBA> VisualObject::getColors() const {
+     313           4 :   return colors_;
+     314             : }
+     315             : //}
+     316             : 
+     317             : }  // 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..b910f327db --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html @@ -0,0 +1,100 @@ + + + + + + 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 + + +
+ 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..af38cbc0ca11fc9d3c74d943d142de04550935b1 GIT binary patch literal 1166 zcmV;91abR`P)0GscesKPK=aG>FH;9Jv{QJ-20V9)iEvnYI9TmjjMN#B7W1 z`m{ePt})Nh+_qPqYlP&P+1n=;XbdL_3v{xx4*{|y`5tqZM}*qf!f^;J*mwZXOapYc zI-1!N+mPP%=jEgZ7Mi9u9(xWh%^(#(OcA^vB%b`M}UQsl0E zEh2aAE9Yl`tPZaL_F^&DqsABhhEBjhD0-xY=y54R-w-~0t8Nu zktT6X5X}0V3Z!ioXV19UvFu3Mgl-^s)Ietpa2P(YSfEaa*vu%$o-iDXlJbOeEl>M> zQ)%X$98HsI)-3}gCz5#1$gxj3%W*{cFZs11PkEtM&ws4`ck{=kDNw_cmoMj0QEP$j z-ECOwy0UpI^8_*wH@0^MxVq63umu)AS7E4@nF!kix*li&tITi!CVND67n2NYjY2)E z&>cMn&YRijBZ9rnH0Kb4nhAllG!p}CU9@KLxO7hy?B@!fq5r4vOad$N$3(xMu+OBf z-)lX;AxMovm1I4okm^`ZLYQc2q2`*fo8HbPGZTO(%}mIWL^F$);|^DtnN~b?8g7K1W5P}>H?h``R#~n- z4_7Z3-WDzbPg?lbNeeFt*tK8R@^LK%eFMCWLI9pMieH*?P0WWkLkuKHqrheci34JD zq}501EeFWJIs(qDb76VIPNOsnJeYZnn5FoIKd9q9o}?i&nsrW-j%cchG7r1ZV;}gA zcrXbQ*SR#5nZ>3)@QytK@LcSX=B0>Ys&mK*m*Fz|l8{A*^y2!7*pvLL8!3h%<*i7; zOvj%N0M#ypWPRZlH`m-r3(qk_qO$kt$c&2e?XCmh*;JwLu|#L>W%k(1QiUXHx$qGu z{|a!f5LYiYVIQQGT2*rao$sVTlp8Sk>w%r1cO9^^fK%^%IVS(Jb^j)#bJ*u~IU9;&+wE;SsN@M?=+~elCG3y`S^{HXI+Eg5t9y`lfVLxtv zt*!voD`!{jbyaM gbNzfgRewzKA60I + + + + + + 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:2023-12-06 07:59:45Functions: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&)31424
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)31424
+
+
+ + + +
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..698a865b82 --- /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:2023-12-06 07:59:45Functions: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&)31424
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&)31424
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..f2ac8332b5 --- /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:2023-12-06 07:59:45Functions: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       31424 :     geometry_msgs::Quaternion fromEigen(const Eigen::Quaterniond& what)
+      48             :     {
+      49       31424 :       geometry_msgs::Quaternion q;
+      50       31424 :       q.x = what.x();
+      51       31424 :       q.y = what.y();
+      52       31424 :       q.z = what.z();
+      53       31424 :       q.w = what.w();
+      54       31424 :       return q;
+      55             :     }
+      56             :     
+      57       31424 :     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       31424 :       Eigen::Quaterniond q;
+      61       31424 :       q.x() = what.x;
+      62       31424 :       q.y() = what.y;
+      63       31424 :       q.z() = what.z;
+      64       31424 :       q.w() = what.w;
+      65       31424 :       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..b5143805ff --- /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:6544514.6 %
Date:2023-12-06 07:59:45Functions:149414.9 %
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 +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
<unnamed>51.6 %33 / 6443.8 %7 / 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..3a5afbb0a5 --- /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:6544514.6 %
Date:2023-12-06 07:59:45Functions:149414.9 %
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 +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
<unnamed>51.6 %33 / 6443.8 %7 / 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..52ef70cef8 --- /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:6544514.6 %
Date:2023-12-06 07:59:45Functions:149414.9 %
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 +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
<unnamed>51.6 %33 / 6443.8 %7 / 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..21bc08d3a4 --- /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:6544514.6 %
Date:2023-12-06 07:59:45Functions:149414.9 %
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 +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 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..154b3ba848 --- /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:6544514.6 %
Date:2023-12-06 07:59:45Functions:149414.9 %
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 +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 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..492b73090c --- /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:6544514.6 %
Date:2023-12-06 07:59:45Functions:149414.9 %
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 +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 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..1853e48531 --- /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:336451.6 %
Date:2023-12-06 07:59:45Functions:71643.8 %
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::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_lib::geometry::haversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::rotationBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::angleaxisBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::cross(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>)3
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)13199
mrs_lib::geometry::quaternionFromHeading(double)31430
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)65240
+
+
+ + + +
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..4ed2c6948d --- /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:336451.6 %
Date:2023-12-06 07:59:45Functions:71643.8 %
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&)13199
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)31430
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&)0
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)65240
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..409fe23a50 --- /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:336451.6 %
Date:2023-12-06 07:59:45Functions:71643.8 %
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       13199 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30       13199 :       const double sin_12 = vec1.cross(vec2).norm();
+      31       13199 :       const double cos_12 = vec1.dot(vec2);
+      32       13199 :       const double angle = std::atan2(sin_12, cos_12);
+      33       13199 :       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       31430 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94       62860 :       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           0 :     double dist(const vec2_t& a, const vec2_t& b)
+     171             :     {
+     172             : 
+     173           0 :       return (a - b).norm();
+     174             :     }
+     175             : 
+     176       65240 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179       65240 :       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..8118e0b3b14707468e51b536c53b3fd75d30c55a GIT binary patch literal 760 zcmVi;~Z+NCa7s z+`}tGDv?9e60itJR2Ge^S6M%{VTVw|6uq!RendT&)SOb2s(}qQI&B19>_ghFI?_X) z6RT7W(WGTv8Dkq`=DgWs7srZ30E7Q8MF6eJeOGGzZKuW-dWJ7u>q092M(9I?Gexta z+TV2&zH6pi)<~akQ_0g~vT1@dEf>9%shPsHh2jZ5U)YUFpY7 z^avxVt{SSy>q;-nle{OUR(Thy0Z@2d`nn5PT0NQk`4CvO9!qR0ps60)^}rgq;;8bH zvvKqMxDbmq9zsrlQwcwjz=1^q&+{ltV52wkipM&7-W9H6It$5n;1PQd_Ru1LP236ElC+?{%CJiJQh6EIX7SMfH_^e+m$DxOXz zoe-MTN8@>djMqoYtfxldT^Po-klA)U&Ed@rdt6?_($td8CeSmH@G<)-MPtNYu2X66 zNH8t33c;>l!>xMo<)ZoL-}i(xTE~RfF3)fdLN3xt;zEf=r!xY+ zC=^QhCv7xQP-q`C_PB;e@Cx!@oCB=4Q3WQPfG$ZcmB6ZL>-{zE(jmX0s@GU8hh5 + + + + + + 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:2023-12-06 07:59:45Functions: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..ddd11ee2e4 --- /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:2023-12-06 07:59:45Functions: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..ab8f4abb0e --- /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:2023-12-06 07:59:45Functions: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:2023-12-06 07:59:45Functions: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..31aaf479c1 --- /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:2023-12-06 07:59:45Functions: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..beb733e02d --- /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:2023-12-06 07:59:45Functions: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..839bed72d3 --- /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:2023-12-06 07:59:45Functions: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..83b409c972 --- /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:2023-12-06 07:59:45Functions: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..82e6c67758 --- /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:2023-12-06 07:59:45Functions: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..5102e42ab1 --- /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:2023-12-06 07:59:45Functions: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..0bf8030246 --- /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:2023-12-06 07:59:45Functions: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..2d96c84974 --- /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:2023-12-06 07:59:45Functions: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:2023-12-06 07:59:45Functions: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..404b942985 --- /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:2023-12-06 07:59:45Functions: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..f35ec75ce0 --- /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:2023-12-06 07:59:45Functions: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..3c361b4a34 --- /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:2023-12-06 07:59:45Functions: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..0385ec0c27 --- /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:2023-12-06 07:59:45Functions: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..9d265ec5d9 --- /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:2023-12-06 07:59:45Functions: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..cc36581850 --- /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:2023-12-06 07:59:45Functions: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::MedianFilter(mrs_lib::MedianFilter&&)11
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)12
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)14
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::check(double)12236
mrs_lib::MedianFilter::median() const12252
mrs_lib::MedianFilter::full() const13217
mrs_lib::MedianFilter::add(double)13222
+
+
+ + + +
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..55d29fdeb1 --- /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:2023-12-06 07:59:45Functions: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)13222
mrs_lib::MedianFilter::check(double)12236
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)11
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)14
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)12
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const13217
mrs_lib::MedianFilter::median() const12252
+
+
+ + + +
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..7673316f09 --- /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:2023-12-06 07:59:45Functions: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          14 :   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          14 :       m_max_diff(max_diff)
+      12             :   {
+      13          14 :     m_buffer.set_capacity(buffer_length);
+      14          14 :     m_buffer_sorted.reserve(buffer_length);
+      15          14 :   }
+      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          11 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33          11 :     *this = other;
+      34          11 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39          12 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41          12 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43          12 :     m_buffer = other.m_buffer;
+      44          12 :     m_buffer_sorted = other.m_buffer_sorted;
+      45          12 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48          12 :     m_min_valid = other.m_min_valid;
+      49          12 :     m_max_valid = other.m_max_valid;
+      50          12 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52          24 :     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       13222 :   void MedianFilter::add(const double value)
+      74             :   {
+      75       26444 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77       13222 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79       13222 :     m_median = std::nullopt;
+      80       13222 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84       12236 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86       12236 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88       12236 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89       24472 :     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       13217 :   bool MedianFilter::full() const
+     113             :   {
+     114       26434 :     std::scoped_lock lck(m_mtx);
+     115       26434 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120       12252 :   double MedianFilter::median() const
+     121             :   {
+     122       24504 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124       12252 :     if (m_median.has_value())
+     125          17 :       return m_median.value();
+     126             :   
+     127             :     // check if there are even any numbers to calculate the median from
+     128       12235 :     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       12231 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137       12231 :     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       12231 :     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       12231 :     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       12231 :     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       12231 :     if (even_set)
+     148       12215 :       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       12231 :     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:2023-12-06 07:59:45Functions: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..96abcf4e74 --- /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:2023-12-06 07:59:45Functions: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..da7d6bbb4d --- /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:2023-12-06 07:59:45Functions: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..50eff0e84e --- /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:2023-12-06 07:59:45Functions: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..5fb1f1bb71 --- /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:2023-12-06 07:59:45Functions: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..6afca46b4f --- /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:2023-12-06 07:59:45Functions: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..e961cd4921 --- /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:2023-12-06 07:59:45Functions: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)1793
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5948
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const39708
+
+
+ + + +
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..174d46f136 --- /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:2023-12-06 07:59:45Functions: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&)5948
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)1793
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const39708
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..6d0ee99a4e --- /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:2023-12-06 07:59:45Functions: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        1793 :   ParamProvider::ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam)
+      14        1793 :   : m_nh(nh), m_node_name(std::move(node_name)), m_use_rosparam(use_rosparam)
+      15             :   {
+      16        1793 :   }
+      17             : 
+      18        5948 :   bool ParamProvider::addYamlFile(const std::string& filepath)
+      19             :   {
+      20             :     try
+      21             :     {
+      22       11896 :       const auto loaded_yaml = YAML::LoadFile(filepath);
+      23        5948 :       YAML::Node root;
+      24        5948 :       root["root"] = loaded_yaml;
+      25        5948 :       m_yamls.emplace_back(root);
+      26        5948 :       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       39708 :   std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+      65             :   {
+      66      203540 :     for (const auto& yaml : m_yamls)
+      67             :     {
+      68             :       // Try to load the parameter sequentially as a map.
+      69      196490 :       auto cur_node_it = std::cbegin(yaml);
+      70             :       // The root should always be a pam
+      71      196490 :       if (!cur_node_it->second.IsMap())
+      72        5260 :         continue;
+      73             : 
+      74      191230 :       bool loaded = true;
+      75             :       {
+      76      191230 :         constexpr char delimiter = '/';
+      77      191230 :         auto substr_start = std::cbegin(param_name);
+      78      191230 :         auto substr_end = substr_start;
+      79      256958 :         do
+      80             :         {
+      81      448188 :           substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+      82             :           // why can't substr or string_view take iterators? :'(
+      83      448183 :           const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+      84      448183 :           const auto count = std::distance(substr_start, substr_end);
+      85      448183 :           const std::string param_substr = param_name.substr(start_pos, count);
+      86      448189 :           substr_start = substr_end+1;
+      87             : 
+      88      448188 :           bool found = false;
+      89     1853885 :           for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+      90             :           {
+      91      957486 :             if (node_it->first.as<std::string>() == param_substr)
+      92             :             {
+      93      289617 :               cur_node_it = node_it;
+      94      289617 :               found = true;
+      95      289617 :               break;
+      96             :             }
+      97             :           }
+      98             : 
+      99      448187 :           if (!found)
+     100             :           {
+     101      158572 :             loaded = false;
+     102      158572 :             break;
+     103             :           }
+     104             :         }
+     105      289615 :         while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+     106             :       }
+     107             : 
+     108      191229 :       if (loaded)
+     109             :       {
+     110       65316 :         return cur_node_it->second;
+     111             :       }
+     112             :     }
+     113             : 
+     114        7050 :     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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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 +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
<unnamed>38.6 %39 / 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..b588b285cd --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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 +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
<unnamed>38.6 %39 / 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..4a8d640158 --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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 +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
<unnamed>38.6 %39 / 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..bbdbdbf691 --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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 +
38.6%38.6%
+ 		38.6 %39 / 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..532b389b22 --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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 +
38.6%38.6%
+ 		38.6 %39 / 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..f7ff9d485b --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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 +
38.6%38.6%
+ 		38.6 %39 / 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..64c5c7d748 --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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)589
mrs_lib::Profiler::Profiler()589
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)589
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)203419
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)203501
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)666644
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)666652
mrs_lib::Routine::end()870126
mrs_lib::Routine::~Routine()870128
+
+
+ + + +
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..2c5cf7199c --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Routine::end()870126
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)666644
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)203501
mrs_lib::Routine::~Routine()870128
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)666652
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)203419
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)589
mrs_lib::Profiler::Profiler()589
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)589
+
+
+ + + +
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..c6a215a0ff --- /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:3910138.6 %
Date:2023-12-06 07:59:45Functions: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         589 : Profiler::Profiler() {
+      11         589 : }
+      12             : 
+      13         589 : Profiler::Profiler(ros::NodeHandle& nh, std::string _node_name_, bool profiler_enabled) {
+      14             : 
+      15         589 :   this->nh_                = std::make_shared<ros::NodeHandle>(nh);
+      16         589 :   this->_node_name_        = _node_name_;
+      17         589 :   this->_profiler_enabled_ = profiler_enabled;
+      18             : 
+      19         589 :   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         589 :   ROS_INFO("[%s]: profiler initialized", _node_name_.c_str());
+      25             : 
+      26         589 :   this->is_initialized_ = true;
+      27         589 : }
+      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         589 : Profiler& Profiler::operator=(const Profiler& other) {
+      43             : 
+      44         589 :   if (this == &other) {
+      45           0 :     return *this;
+      46             :   }
+      47             : 
+      48         589 :   this->is_initialized_    = other.is_initialized_;
+      49         589 :   this->nh_                = other.nh_;
+      50         589 :   this->_node_name_        = other._node_name_;
+      51         589 :   this->_profiler_enabled_ = other._profiler_enabled_;
+      52             : 
+      53         589 :   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         589 :   return *this;
+      59             : }
+      60             : 
+      61             : //}
+      62             : 
+      63             : /* Profiler::registerRoutine() for periodic //{ */
+      64             : 
+      65      203419 : Routine Profiler::createRoutine(std::string name, double expected_rate, double threshold, ros::TimerEvent event) {
+      66             : 
+      67      203419 :   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      666652 : Routine Profiler::createRoutine(std::string name) {
+      75             : 
+      76      666652 :   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      203501 : Routine::Routine(std::string name, std::string node_name, double expected_rate, double threshold, std::shared_ptr<ros::Publisher> publisher,
+      86      203501 :                  std::shared_ptr<std::mutex> mutex_publisher, bool profiler_enabled, ros::TimerEvent event) {
+      87             : 
+      88      203116 :   if (!profiler_enabled) {
+      89      203125 :     return;
+      90             :   }
+      91             : 
+      92           1 :   _threshold_ = threshold;
+      93             : 
+      94           1 :   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      666644 : Routine::Routine(std::string name, std::string node_name, std::shared_ptr<ros::Publisher> publisher, std::shared_ptr<std::mutex> mutex_publisher,
+     141      666644 :                  bool profiler_enabled) {
+     142             : 
+     143      666330 :   if (!profiler_enabled) {
+     144      666344 :     return;
+     145             :   }
+     146             : 
+     147           0 :   this->publisher_       = publisher;
+     148           0 :   this->mutex_publisher_ = mutex_publisher;
+     149             : 
+     150           0 :   this->_routine_name_  = name;
+     151           0 :   msg_out_.routine_name = name;
+     152             : 
+     153           0 :   this->_node_name_  = node_name;
+     154           0 :   msg_out_.node_name = node_name;
+     155             : 
+     156           0 :   this->_profiler_enabled_ = profiler_enabled;
+     157             : 
+     158           0 :   msg_out_.is_periodic   = false;
+     159           0 :   msg_out_.expected_rate = 0;
+     160             : 
+     161           0 :   msg_out_.stamp      = ros::Time::now();
+     162           0 :   msg_out_.duration   = 0;
+     163           0 :   msg_out_.iteration  = this->iteration_++;
+     164           0 :   msg_out_.event      = mrs_msgs::ProfilerUpdate::START;
+     165           0 :   msg_out_.real_start = msg_out_.stamp.toSec();
+     166             : 
+     167           0 :   execution_start_ = ros::Time::now();
+     168             : 
+     169             :   {
+     170           0 :     std::scoped_lock lock(*mutex_publisher_);
+     171             : 
+     172             :     try {
+     173           0 :       publisher_->publish(mrs_msgs::ProfilerUpdateConstPtr(new mrs_msgs::ProfilerUpdate(msg_out_)));
+     174             :     }
+     175           0 :     catch (...) {
+     176           0 :       ROS_ERROR("Exception caught during publishing topic %s.", publisher_->getTopic().c_str());
+     177             :     }
+     178             :   }
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* end() //{ */
+     184             : 
+     185      870126 : void Routine::end(void) {
+     186             : 
+     187      870126 :   if (!_profiler_enabled_) {
+     188      870049 :     return;
+     189             :   }
+     190             : 
+     191          77 :   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      870063 : Routine::~Routine() {
+     213             : 
+     214      870128 :   this->end();
+     215      869593 : }
+     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..ee7178374126c2cf561db47acab37502e0a548ac GIT binary patch literal 841 zcmV-P1GfB$P)#D`p{NA#Z`qPgMIV2pE;X_TZh;%%g;)Eg0W6tn(>_b{M%ptm^s zPsg)q+5y15pa>VDBIjUM^aDy3Z+Jb<$%>I;+AYgBinU)V3l>mZ4|j@0y2ULj(a#mgsjs=aZ}6&9Z8Pk6yah}WOJ1*I+E_4GKMZE z&}CCi@w`n$xYd|Kg= zHMueQ!@7o4b~b>R@`?G)fZwK(hXLPJiu(aK55p zgu2R#)zD5LxD@(Ku^O88_BIwh2T(x+GUw1WXB;Nsg=wU?UUf%mSD-QPk;k~^TB1O^ zTpyWaq1!kkO^AAn7Nvw{=bA+e!{b(>*djvO)(l_&vVP5G*FzglA7G^FjWa~AkFr-8 zo~>i6&=(S)ouu)sin{oCXWU6vz?ofULRo{~T4;5^AK1}+hLIvVbAMa&jLv`{I+KYd zzX_;s?EgQVNwaYgcbfRl^^%Vp&?Jy8$%n$34f(jdTwws2QSfvo-*t!Vh>j6)6n2_1 z&|EMv@4<@99%-%D?Wn@rG*1-YrYTwMeYb3Ach-I@rE5p!uB*@t|9ok0!0i+OZj4Ik zT%^SEi0dOL(shkXib_cq%k@!-{}w43`YlpzWL6cF3;Sm0itWD}9{NG`k0k#9NVwS- T>2taX00000NkvXXu0mjfhUbsa literal 0 HcmV?d00001 diff --git a/mrs_lib/src/safety_zone/index-detail-sort-f.html b/mrs_lib/src/safety_zone/index-detail-sort-f.html new file mode 100644 index 0000000000..1af0d8dd84 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2023-12-06 07:59:45Functions: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..4d88174346 --- /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:2023-12-06 07:59:45Functions: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..6727fede6a --- /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:2023-12-06 07:59:45Functions: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..24f68aff6c --- /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:2023-12-06 07:59:45Functions: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..97d3dbaaff --- /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:2023-12-06 07:59:45Functions: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..937fc5390a --- /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:2023-12-06 07:59:45Functions: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..4b2d0484e8 --- /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:2023-12-06 07:59:45Functions: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>)206
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)236
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>)236
+
+
+ + + +
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..0df022985d --- /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:2023-12-06 07:59:45Functions: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>)206
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)236
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>)236
+
+
+ + + +
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..5684bc3945 --- /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:2023-12-06 07:59:45Functions: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         206 : 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         206 :   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         236 : Intersection::Intersection(bool intersect, bool parallel, Eigen::RowVector2d point) : point(std::move(point)), parallel(parallel), intersect(intersect){}
+      16             : 
+      17             : /* sectionIntersect() //{ */
+      18             : 
+      19         236 : Intersection sectionIntersect(Eigen::RowVector2d start1, Eigen::RowVector2d end1, Eigen::RowVector2d start2, Eigen::RowVector2d end2) {
+      20         236 :   Eigen::RowVector2d vector1 = end1 - start1;
+      21         236 :   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         236 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+      29         236 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector2(0);
+      30             : 
+      31         236 :   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         206 :   double             scale1 = difference_cross / cross;
+      48         206 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+      49         206 :   double             scale2 = getScale(start2, vector2, point);
+      50             : 
+      51         206 :   if (0 <= scale1 && scale1 <= 1 && 0 <= scale2 && scale2 <= 1) {
+      52           0 :     return Intersection{true, false, point};
+      53             :   }
+      54         206 :   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..043e5b5645 --- /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:2023-12-06 07:59:45Functions: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..b28d43cd8f --- /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:2023-12-06 07:59:45Functions: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..5ac959b974 --- /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:2023-12-06 07:59:45Functions: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..2b369644ab --- /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:2023-12-06 07:59:45Functions: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..1c7e36e024 --- /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:2023-12-06 07:59:45Functions: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..bad2018fdb --- /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:2023-12-06 07:59:45Functions: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..ee2ee1ac6a --- /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:2023-12-06 07:59:45Functions:4850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Polygon::inflateSelf(double)0
mrs_lib::Polygon::isClockwise()0
mrs_lib::lineIntersectGivenVector(Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, 1, 2, 1, 1, 2>)0
mrs_lib::movePoint(Eigen::Matrix<double, 1, 2, 1, 1, 2>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2>)0
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)39
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)59
mrs_lib::Polygon::isPointInside(double, double)1789
mrs_lib::Polygon::getPointMessageVector(double)6910
+
+
+ + + +
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..55d146bdec --- /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:2023-12-06 07:59:45Functions: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)1789
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)59
mrs_lib::Polygon::getPointMessageVector(double)6910
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)39
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..ce1fd1fbde --- /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:2023-12-06 07:59:45Functions: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          39 : Polygon::Polygon(const Eigen::MatrixXd vertices) : vertices(vertices) {
+      10             : 
+      11          39 :   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          39 :   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          39 :   Eigen::RowVector2d edge1;
+      21          39 :   Eigen::RowVector2d edge2 = vertices.row(1) - vertices.row(0);
+      22         195 :   for (int i = 0; i < vertices.rows(); ++i) {
+      23         156 :     edge1 = edge2;
+      24         156 :     edge2 = vertices.row((i + 2) % vertices.rows()) - vertices.row((i + 1) % vertices.rows());
+      25             : 
+      26         156 :     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         156 :     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          39 : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* isPointInside() //{ */
+      40             : 
+      41        1789 : bool Polygon::isPointInside(const double px, const double py) {
+      42             : 
+      43        1789 :   int count = 0;
+      44             : 
+      45             :   // Cast a horizontal ray and see how many times it intersects all the edges
+      46        8945 :   for (int i = 0; i < vertices.rows(); ++i) {
+      47        7156 :     double v1x = vertices(i, 0);
+      48        7156 :     double v1y = vertices(i, 1);
+      49        7156 :     double v2x = vertices((i + 1) % vertices.rows(), 0);
+      50        7156 :     double v2y = vertices((i + 1) % vertices.rows(), 1);
+      51             : 
+      52        7156 :     if (v1y > py && v2y > py)
+      53        1712 :       continue;
+      54        5444 :     if (v1y <= py && v2y <= py)
+      55        2020 :       continue;
+      56        3424 :     double intersect_x    = (v1y == v2y) ? v1x : (py - v1y) * (v2x - v1x) / (v2y - v1y) + v1x;
+      57        3424 :     bool   does_intersect = intersect_x > px;
+      58        3424 :     if (does_intersect)
+      59        1712 :       ++count;
+      60             :   }
+      61             : 
+      62        1789 :   return count % 2;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* doesSectionIntersect() //{ */
+      68             : 
+      69          59 : bool Polygon::doesSectionIntersect(const double startX, const double startY, const double endX, const double endY) {
+      70             : 
+      71          59 :   Eigen::RowVector2d start{startX, startY};
+      72          59 :   Eigen::RowVector2d end{endX, endY};
+      73             : 
+      74         295 :   for (int i = 0; i < vertices.rows(); ++i) {
+      75             : 
+      76         236 :     Eigen::RowVector2d edgeStart = vertices.row(i);
+      77         236 :     Eigen::RowVector2d edgeEnd   = vertices.row((i + 1) % vertices.rows());
+      78             : 
+      79         236 :     if (sectionIntersect(start, end, edgeStart, edgeEnd).intersect) {
+      80           0 :       return true;
+      81             :     }
+      82             :   }
+      83             : 
+      84          59 :   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        6910 : std::vector<geometry_msgs::Point> Polygon::getPointMessageVector(const double z) {
+     194        6910 :   std::vector<geometry_msgs::Point> points(vertices.rows());
+     195       34550 :   for (int i = 0; i < vertices.rows(); ++i) {
+     196       27640 :     points[i].x = vertices(i, 0);
+     197       27640 :     points[i].y = vertices(i, 1);
+     198       27640 :     points[i].z = z;
+     199             :   }
+     200             : 
+     201        6910 :   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..83792b37ae --- /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:2023-12-06 07:59:45Functions:5683.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)39
mrs_lib::SafetyZone::~SafetyZone()39
mrs_lib::SafetyZone::isPathValid(double, double, double, double)59
mrs_lib::SafetyZone::isPointValid(double, double)1789
mrs_lib::SafetyZone::getBorder()3455
+
+
+ + + +
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..a6ef5c6644 --- /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:2023-12-06 07:59:45Functions: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)59
mrs_lib::SafetyZone::isPointValid(double, double)1789
mrs_lib::SafetyZone::getBorder()3455
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)39
mrs_lib::SafetyZone::~SafetyZone()39
+
+
+ + + +
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..212e6905b9 --- /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:2023-12-06 07:59:45Functions: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          78 : SafetyZone::~SafetyZone() {
+      17          39 :   delete outerBorder;
+      18          39 : }
+      19             : 
+      20             : //}
+      21             : 
+      22             : /* SafetyZone() //{ */
+      23             : 
+      24          39 : SafetyZone::SafetyZone(const Eigen::MatrixXd& outerBorderMatrix) {
+      25             : 
+      26             :   try {
+      27          39 :     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          39 : }
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* isPointValid() //{ */
+      43             : 
+      44        1789 : bool SafetyZone::isPointValid(const double px, const double py) {
+      45             : 
+      46        1789 :   if (!outerBorder->isPointInside(px, py)) {
+      47          77 :     return false;
+      48             :   }
+      49             : 
+      50        1712 :   return true;
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* isPathValid() //{ */
+      56             : 
+      57          59 : bool SafetyZone::isPathValid(const double p1x, const double p1y, const double p2x, const double p2y) {
+      58             : 
+      59          59 :   if (outerBorder->doesSectionIntersect(p1x, p1y, p2x, p2y)) {
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63          59 :   return true;
+      64             : }
+      65             : 
+      66             : //}
+      67             : 
+      68             : /* getBorder() //{ */
+      69             : 
+      70        3455 : Polygon SafetyZone::getBorder() {
+      71        3455 :   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:2023-12-06 07:59:45Functions: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..03b802d907 --- /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:2023-12-06 07:59:45Functions: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..97f9e53493 --- /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:2023-12-06 07:59:45Functions: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..859efb9314 --- /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:2023-12-06 07:59:45Functions: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..93b6205f8d --- /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:2023-12-06 07:59:45Functions: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..457ab65391 --- /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:2023-12-06 07:59:45Functions: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..fc191a8b7e --- /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:2023-12-06 07:59:45Functions: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)294
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()294
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1425027
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)1768726
mrs_lib::ScopeTimer::~ScopeTimer()1769320
+
+
+ + + +
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..5b7ed19dfe --- /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:2023-12-06 07:59:45Functions: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&)1425027
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)1768726
mrs_lib::ScopeTimer::~ScopeTimer()1769320
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)294
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()294
+
+
+ + + +
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..c21c083607 --- /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:2023-12-06 07:59:45Functions: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         294 : ScopeTimerLogger::ScopeTimerLogger(const std::string& logfile, const bool enable_logging, const int log_precision)
+      10         294 :     : _logging_enabled_(enable_logging), _log_filepath_(logfile) {
+      11             : 
+      12         294 :   if (!_logging_enabled_) {
+      13         294 :     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         294 : ScopeTimerLogger::~ScopeTimerLogger() {
+      47         294 :   if (_logging_enabled_) {
+      48           0 :     ROS_DEBUG("[%s]: ScopeTimerLogger: closing logstream.", ros::this_node::getName().c_str());
+      49           0 :     _logstream_.close();
+      50             :   }
+      51         294 : }
+      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     1768726 : ScopeTimer::ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable)
+     104     1768726 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(ros::Duration(0.0)), _logger_(scope_timer_logger) {
+     105             : 
+     106     1764922 :   checkpoints.push_back(time_point("timer start"));
+     107             : 
+     108     1764299 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     109     1764299 : }
+     110             : 
+     111             : //}
+     112             : 
+     113             : /* ScopeTimer::checkpoint() //{ */
+     114             : 
+     115     1425027 : void ScopeTimer::checkpoint(const std::string& label) {
+     116             : 
+     117     1425027 :   if (_enable_print_or_log) {
+     118           0 :     checkpoints.push_back(time_point(label));
+     119             :   }
+     120     1425027 : }
+     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     5306428 : ScopeTimer::~ScopeTimer() {
+     136             : 
+     137     1769320 :   if (!_enable_print_or_log) {
+     138     1769325 :     return;
+     139             :   }
+     140             : 
+     141           7 :   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     1768611 : }
+     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:2023-12-06 07:59:45Functions: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..89ad826601 --- /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:2023-12-06 07:59:45Functions: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..45db4bd0bb --- /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:2023-12-06 07:59:45Functions: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..80320589fa --- /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:2023-12-06 07:59:45Functions: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..db6b59454c --- /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:2023-12-06 07:59:45Functions: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..aa2292a3b0 --- /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:2023-12-06 07:59:45Functions: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..0e3054fe43 --- /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:2023-12-06 07:59:45Functions: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&)13
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)16
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)16
mrs_lib::TimeoutManager::started(unsigned long)1597
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)3509
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)17264
+
+
+ + + +
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..308701f91a --- /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:2023-12-06 07:59:45Functions: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)16
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)3509
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)17264
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)16
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)1597
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&)13
+
+
+ + + +
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..86b1fafc02 --- /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:2023-12-06 07:59:45Functions: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          13 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7          13 :     : m_last_id(0)
+       8             :   {
+       9          13 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10          13 :   }
+      11             : 
+      12             : 
+      13          16 :   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          32 :     std::scoped_lock lck(m_mtx);
+      16          16 :     const auto new_id = m_timeouts.size();
+      17          16 :     const timeout_info_t new_info = {oneshot, autostart, callback, timeout, last_reset, last_reset};
+      18          16 :     m_timeouts.push_back(new_info);
+      19          32 :     return new_id;
+      20             :   }
+      21             : 
+      22       17264 :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      23             :   {
+      24       17264 :     std::scoped_lock lck(m_mtx);
+      25       17264 :     m_timeouts.at(id).last_reset = time;
+      26       17264 :   }
+      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          16 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      35             :   {
+      36          16 :     std::scoped_lock lck(m_mtx);
+      37          16 :     m_timeouts.at(id).started = true;
+      38          16 :     m_timeouts.at(id).last_reset = time;
+      39          16 :   }
+      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        1597 :   bool TimeoutManager::started(const timeout_id_t id)
+      76             :   {
+      77        1597 :     std::scoped_lock lck(m_mtx);
+      78        3194 :     return m_timeouts.at(id).started;
+      79             :   }
+      80             : 
+      81        3509 :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      82             :   {
+      83        3509 :     const auto now = ros::Time::now();
+      84             : 
+      85        7018 :     std::scoped_lock lck(m_mtx);
+      86       13300 :     for (auto& timeout_info : m_timeouts)
+      87             :     {
+      88             :       // don't worry, this'll get optimized out by the compiler
+      89        9791 :       const bool started = timeout_info.started;
+      90        9791 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      91        9791 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      92        9791 :       if (started && last_reset_timeout && last_callback_timeout)
+      93             :       {
+      94        1644 :         timeout_info.callback(timeout_info.last_reset);
+      95        1644 :         timeout_info.last_callback = now;
+      96             :         // if the timeout is oneshot, pause it
+      97        1644 :         if (timeout_info.oneshot)
+      98           0 :           timeout_info.started = false;
+      99             :       }
+     100             :     }
+     101        3509 :   }
+     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:2023-12-06 07:59:45Functions: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..5c0707ebbc --- /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:2023-12-06 07:59:45Functions: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..786a3d88d2 --- /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:2023-12-06 07:59:45Functions: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..b63dd039fd --- /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:2023-12-06 07:59:45Functions: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..8991f3910e --- /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:2023-12-06 07:59:45Functions: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..e44753cf30 --- /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:2023-12-06 07:59:45Functions: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..c139154e2c --- /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:2023-12-06 07:59:45Functions: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()216
+
+
+ + + +
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..31ae37c16a --- /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:2023-12-06 07:59:45Functions: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()216
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..a790253d72 --- /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:2023-12-06 07:59:45Functions: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         216 : bool ROSTimer::running()
+      50             : {
+      51         432 :   std::scoped_lock lock(mutex_timer_);
+      52             : 
+      53         216 :   if (timer_)
+      54         216 :     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..ce3ca65c53 --- /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:2023-12-06 07:59:45Functions: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..a66dff419a --- /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:2023-12-06 07:59:45Functions: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..33f78cddf5 --- /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:2023-12-06 07:59:45Functions: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..34a29cfdfc --- /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:2023-12-06 07:59:45Functions: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..1b5d8c9a20 --- /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:2023-12-06 07:59:45Functions: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..1fac4f7fbc --- /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:2023-12-06 07:59:45Functions: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..9a7dcf7108 --- /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:2023-12-06 07:59:45Functions: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()83
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)769543
+
+
+ + + +
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..00d635ccd8 --- /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:2023-12-06 07:59:45Functions: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&)769543
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()83
+
+
+ + + +
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..f2c6d0e0a8 --- /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:2023-12-06 07:59:45Functions: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          83 : TransformBroadcaster::TransformBroadcaster() {
+       8          83 :   broadcaster_ = tf2_ros::TransformBroadcaster();
+       9          83 : }
+      10             : //}
+      11             : 
+      12             : /* sendTransform (const geometry_msgs::TransformStamped &transform) //{ */
+      13      769543 : void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) {
+      14     1538677 :   std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      15      768748 :   if (last_messages_.count(frames_from_to) > 0) {
+      16      768892 :     if (transform.header.stamp > last_messages_[frames_from_to]) {
+      17      705980 :       broadcaster_.sendTransform(transform);
+      18      706716 :       last_messages_[frames_from_to] = transform.header.stamp;
+      19             :     } else {
+      20       61998 :       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         906 :     std::pair<std::string, ros::Time> new_pair;
+      25         443 :     new_pair.first  = frames_from_to;
+      26         443 :     new_pair.second = transform.header.stamp;
+      27         443 :     broadcaster_.sendTransform(transform);
+      28         443 :     last_messages_.insert(new_pair);
+      29             :   }
+      30      769595 : }
+      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:2023-12-06 07:59:45Functions: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..c3d4cb580b --- /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:2023-12-06 07:59:45Functions: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..0e0d599503 --- /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:2023-12-06 07:59:45Functions: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..8cfe00a4aa --- /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:2023-12-06 07:59:45Functions: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..e9db742725 --- /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:2023-12-06 07:59:45Functions: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..fbb8f0ee66 --- /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:2023-12-06 07:59:45Functions: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..16f3dc98f9 --- /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:2023-12-06 07:59:45Functions: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&)252
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)490
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)490
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)492
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)990
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&)7067
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)31424
mrs_lib::Transformer::setLatLon(double, double)60929
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&)601847
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2487842
+
+
+ + + +
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..424cc88494 --- /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:2023-12-06 07:59:45Functions: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&)7067
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&)31424
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)990
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&)601847
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2487842
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&)490
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&)490
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)492
mrs_lib::Transformer::setLatLon(double, double)60929
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)252
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..a21613bb15 --- /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:2023-12-06 07:59:45Functions: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         252 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49         252 :     : 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         252 :   }
+      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        7067 :   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       14135 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82        7068 :     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       14136 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90       14136 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91       14136 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93        7068 :     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       60929 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119       60929 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122       60753 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123       60729 :     got_utm_zone_ = true;
+     124       60545 :   }
+     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       31424 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255       62848 :     geometry_msgs::PoseStamped pose;
+     256       31424 :     pose.header = what.header;
+     257             :   
+     258       31424 :     pose.pose.position.x = what.reference.position.x;
+     259       31424 :     pose.pose.position.y = what.reference.position.y;
+     260       31424 :     pose.pose.position.z = what.reference.position.z;
+     261       31424 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264       62848 :     const auto pose_opt = transformImpl(tf, pose);
+     265       31424 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268       31424 :     pose = pose_opt.value();
+     269             :   
+     270       62848 :     mrs_msgs::ReferenceStamped ret;
+     271       31424 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274       31424 :     ret.reference.position.x = pose.pose.position.x;
+     275       31424 :     ret.reference.position.y = pose.pose.position.y;
+     276       31424 :     ret.reference.position.z = pose.pose.position.z;
+     277       31424 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278       31424 :     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      601847 :   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      601847 :     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      601847 :     if (from_frame.empty() || to_frame.empty())
+     312             :     {
+     313           4 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform to/from an empty frame!", node_name_.c_str());
+     314           4 :       return std::nullopt;
+     315             :     }
+     316             : 
+     317             :     // if the frames are the same, just return an identity transform
+     318      601854 :     if (from_frame == to_frame)
+     319      104698 :       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      549500 :     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      549502 :     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         986 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337         986 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338         493 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341         493 :       frame_to(*tf_opt) = to_frame;
+     342         493 :       return tf_opt;
+     343             :     }
+     344             : 
+     345     1646973 :     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      792633 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352      610582 :     catch (tf2::TransformException& e)
+     353             :     {
+     354      305292 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358      305292 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362      603977 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364       13076 :       catch (tf2::TransformException& e)
+     365             :       {
+     366        6538 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371        6568 :     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        6568 :       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        6568 :     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     2487842 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466     2487842 :     if (frame_id.empty())
+     467       20776 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470     2467150 :     if (prefix_.empty())
+     471     1585713 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474      881400 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475      588696 :       return prefix_ + frame_id;
+     476             : 
+     477      292779 :     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         492 :   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         492 :     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         491 :     geometry_msgs::Point latlon;
+     532         491 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533         491 :     latlon.z = what.z;
+     534         491 :     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         490 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552         490 :     const auto opt = UTMtoLL(what.position, prefix);
+     553         490 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556         490 :     geometry_msgs::Pose ret;
+     557         490 :     ret.position = opt.value();
+     558         490 :     ret.orientation = what.orientation;
+     559         490 :     return ret;
+     560             :   }
+     561             : 
+     562         490 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564         490 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565         490 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568         980 :     geometry_msgs::PoseStamped ret;
+     569         490 :     ret.header.frame_id = prefix + "utm_origin";
+     570         490 :     ret.header.stamp = what.header.stamp;
+     571         490 :     ret.pose = opt.value();
+     572         490 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577         990 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579         990 :     const auto firstof = frame_id.find_first_of('/');
+     580         990 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583         990 :       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..18f740a958 --- /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:2023-12-06 07:59:45Functions: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..e13884cb61 --- /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:2023-12-06 07:59:45Functions: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..ca3f74495c --- /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:2023-12-06 07:59:45Functions: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..0d49a5571d --- /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:2023-12-06 07:59:45Functions: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..fef1d82963 --- /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:2023-12-06 07:59:45Functions: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..b5710030d0 --- /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:2023-12-06 07:59:45Functions: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..adfbe84e25 --- /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:2023-12-06 07:59:45Functions: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>&)160039
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()160040
+
+
+ + + +
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..a2ce115814 --- /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:2023-12-06 07:59:45Functions: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>&)160039
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()160040
+
+
+ + + +
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..ad145bc77d --- /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:2023-12-06 07:59:45Functions: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      160039 : AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>& in)
+       7      160039 :   : variable(in)
+       8             : {
+       9      160039 :   variable = true;
+      10      160040 : }
+      11             : 
+      12      320082 : AtomicScopeFlag::~AtomicScopeFlag()
+      13             : {
+      14      160040 :   variable = false;
+      15      160042 : }
+      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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.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()5
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)6
(anonymous namespace)::ProxyExec0::ProxyExec0()9
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()9
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)14
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)19
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()76
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)232
mrs_uav_autostart::automatic_start::AutomaticStart::speedCheck()1567
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()1567
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()1567
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()1567
mrs_uav_autostart::automatic_start::Topic::getTime()3210
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)3866
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)8956
mrs_uav_autostart::automatic_start::Topic::updateTime()8957
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&) const8957
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)30156
+
+
+ + + +
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..9012c59ef2 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,156 @@ + + + + + + + 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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()9
mrs_uav_autostart::automatic_start::AutomaticStart::speedCheck()1567
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()1567
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)14
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)8956
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()1567
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()1567
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)30156
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)6
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)232
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()3
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()9
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()5
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)3866
mrs_uav_autostart::automatic_start::Topic::updateTime()8957
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()76
mrs_uav_autostart::automatic_start::Topic::getTime()3210
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)19
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&) const8957
+
+
+ + + +
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..b0e52e9230 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,887 @@ + + + + + + + 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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.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/EstimationDiagnostics.h>
+      22             : 
+      23             : #include <topic_tools/shape_shifter.h>
+      24             : 
+      25             : //}
+      26             : 
+      27             : namespace mrs_uav_autostart
+      28             : {
+      29             : 
+      30             : namespace automatic_start
+      31             : {
+      32             : 
+      33             : /* class Topic //{ */
+      34             : 
+      35             : class Topic {
+      36             : private:
+      37             :   std::string topic_name_;
+      38             :   ros::Time   last_time_;
+      39             : 
+      40             : public:
+      41          19 :   Topic(std::string topic_name) : topic_name_(topic_name) {
+      42          19 :     last_time_ = ros::Time(0);
+      43          19 :   }
+      44             : 
+      45        8957 :   void updateTime(void) {
+      46        8957 :     last_time_ = ros::Time::now();
+      47        8957 :   }
+      48             : 
+      49        3210 :   ros::Time getTime(void) {
+      50        3210 :     return last_time_;
+      51             :   }
+      52             : 
+      53          76 :   std::string getTopicName(void) {
+      54          76 :     return topic_name_;
+      55             :   }
+      56             : };
+      57             : 
+      58             : //}
+      59             : 
+      60             : /* class AutomaticStart //{ */
+      61             : 
+      62             : // state machine
+      63             : typedef enum
+      64             : {
+      65             :   STATE_IDLE,
+      66             :   STATE_TAKEOFF,
+      67             :   STATE_FINISHED
+      68             : } LandingStates_t;
+      69             : 
+      70             : const char* state_names[4] = {"IDLING", "TAKEOFF", "FINISHED"};
+      71             : 
+      72             : class AutomaticStart : public nodelet::Nodelet {
+      73             : 
+      74             : public:
+      75             :   virtual void onInit();
+      76             : 
+      77             : private:
+      78             :   ros::NodeHandle   nh_;
+      79             :   std::atomic<bool> is_initialized_ = false;
+      80             : 
+      81             :   std::string _uav_name_;
+      82             :   bool        _simulation_;
+      83             : 
+      84             :   // | --------------------- service clients -------------------- |
+      85             : 
+      86             :   ros::ServiceClient service_client_toggle_control_output_;
+      87             :   ros::ServiceClient service_client_arm_;
+      88             :   ros::ServiceClient service_client_takeoff_;
+      89             :   ros::ServiceClient service_client_eland_;
+      90             :   ros::ServiceClient service_client_validate_reference_;
+      91             : 
+      92             :   // | ----------------------- subscribers ---------------------- |
+      93             : 
+      94             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>     sh_estimation_diag_;
+      95             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>               sh_hw_api_status_;
+      96             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+      97             :   mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>     sh_uav_manager_diag_;
+      98             :   mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>  sh_gazebo_spawner_diag_;
+      99             : 
+     100             :   // | ----------------------- publishers ----------------------- |
+     101             : 
+     102             :   mrs_lib::PublisherHandler<std_msgs::Bool> ph_can_takeoff_;
+     103             : 
+     104             :   // | ----------------------- main timer ----------------------- |
+     105             : 
+     106             :   ros::Timer timer_main_;
+     107             :   void       timerMain(const ros::TimerEvent& event);
+     108             :   double     _main_timer_rate_;
+     109             : 
+     110             :   // | ------------------- hw api diagnostics ------------------- |
+     111             : 
+     112             :   void              callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     113             :   std::atomic<bool> got_hw_api_status_ = false;
+     114             :   std::mutex        mutex_hw_api_status_;
+     115             : 
+     116             :   // | --------------- Gazebo spawner diagnostics --------------- |
+     117             : 
+     118             :   void                               callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg);
+     119             :   std::atomic<bool>                  got_gazebo_spawner_diagnostics = false;
+     120             :   mrs_msgs::GazeboSpawnerDiagnostics gazebo_spawner_diagnostics_;
+     121             :   std::mutex                         mutex_gazebo_spawner_diagnostics_;
+     122             : 
+     123             :   // | ----------------- arm and offboard check ----------------- |
+     124             : 
+     125             :   ros::Time armed_time_;
+     126             :   bool      armed_ = false;
+     127             : 
+     128             :   ros::Time offboard_time_;
+     129             :   bool      offboard_ = false;
+     130             : 
+     131             :   // | ------------------------ routines ------------------------ |
+     132             : 
+     133             :   bool takeoff();
+     134             : 
+     135             :   bool validateReference();
+     136             : 
+     137             :   bool toggleControlOutput(const bool& value);
+     138             :   bool disarm();
+     139             : 
+     140             :   bool isGazeboSimulation(void);
+     141             :   bool topicCheck(void);
+     142             :   bool speedCheck(void);
+     143             :   bool is_gazebo_simulation_ = false;
+     144             : 
+     145             :   // | ---------------------- other params ---------------------- |
+     146             : 
+     147             :   std::string _body_frame_name_;
+     148             :   double      _action_duration_;
+     149             :   double      _pre_takeoff_sleep_;
+     150             :   bool        _handle_takeoff_ = false;
+     151             :   double      _safety_timeout_;
+     152             :   double      _control_output_timeout_;
+     153             : 
+     154             :   // | ---------------------- state machine --------------------- |
+     155             : 
+     156             :   uint current_state = STATE_IDLE;
+     157             :   void changeState(LandingStates_t new_state);
+     158             : 
+     159             :   // | --------------------- velocity check --------------------- |
+     160             : 
+     161             :   bool   _velocity_check_enabled_ = false;
+     162             :   double _velocity_check_max_speed_;
+     163             : 
+     164             :   // | ---------------- generic topic subscribers --------------- |
+     165             : 
+     166             :   bool                     _topic_check_enabled_ = false;
+     167             :   double                   _topic_check_timeout_;
+     168             :   std::vector<std::string> _topic_check_topic_names_;
+     169             : 
+     170             :   std::vector<Topic>           topic_check_topics_;
+     171             :   std::vector<ros::Subscriber> generic_subscriber_vec_;
+     172             : 
+     173             :   // generic callback, for any topic, to monitor its rate
+     174             :   void genericCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name, const int id);
+     175             : };
+     176             : 
+     177             : //}
+     178             : 
+     179             : /* onInit() //{ */
+     180             : 
+     181           9 : void AutomaticStart::onInit() {
+     182             : 
+     183          18 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     184             : 
+     185           9 :   ros::Time::waitForValid();
+     186             : 
+     187           9 :   armed_      = false;
+     188           9 :   armed_time_ = ros::Time(0);
+     189             : 
+     190           9 :   offboard_      = false;
+     191           9 :   offboard_time_ = ros::Time(0);
+     192             : 
+     193          18 :   mrs_lib::ParamLoader param_loader(nh_, "AutomaticStart");
+     194             : 
+     195          18 :   std::string custom_config_path;
+     196             : 
+     197           9 :   param_loader.loadParam("custom_config", custom_config_path);
+     198             : 
+     199           9 :   if (custom_config_path != "") {
+     200           2 :     param_loader.addYamlFile(custom_config_path);
+     201             :   }
+     202             : 
+     203           9 :   param_loader.addYamlFileFromParam("config_private");
+     204           9 :   param_loader.addYamlFileFromParam("config_public");
+     205             : 
+     206           9 :   param_loader.loadParam("uav_name", _uav_name_);
+     207           9 :   param_loader.loadParam("simulation", _simulation_);
+     208             : 
+     209           9 :   param_loader.loadParam("main_timer_rate", _main_timer_rate_);
+     210           9 :   param_loader.loadParam("body_frame_name", _body_frame_name_);
+     211           9 :   param_loader.loadParam("control_output_timeout", _control_output_timeout_);
+     212             : 
+     213           9 :   param_loader.loadParam("safety_timeout", _safety_timeout_);
+     214           9 :   param_loader.loadParam("pre_takeoff_sleep", _pre_takeoff_sleep_);
+     215             : 
+     216           9 :   param_loader.loadParam("handle_takeoff", _handle_takeoff_);
+     217             : 
+     218           9 :   param_loader.loadParam("preflight_check/velocity_check/enabled", _velocity_check_enabled_);
+     219           9 :   param_loader.loadParam("preflight_check/velocity_check/max_speed", _velocity_check_max_speed_);
+     220             : 
+     221           9 :   param_loader.loadParam("preflight_check/topic_check/enabled", _topic_check_enabled_);
+     222           9 :   param_loader.loadParam("preflight_check/topic_check/timeout", _topic_check_timeout_);
+     223           9 :   param_loader.loadParam("preflight_check/topic_check/topics", _topic_check_topic_names_);
+     224             : 
+     225           9 :   if (!param_loader.loadedSuccessfully()) {
+     226           0 :     ROS_ERROR("[AutomaticStart]: Could not load all parameters!");
+     227           0 :     ros::shutdown();
+     228             :   }
+     229             : 
+     230             :   // | ----------------------- subscribers ---------------------- |
+     231             : 
+     232          18 :   mrs_lib::SubscribeHandlerOptions shopts;
+     233           9 :   shopts.nh                 = nh_;
+     234           9 :   shopts.node_name          = "AutomaticStart";
+     235           9 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     236           9 :   shopts.threadsafe         = true;
+     237           9 :   shopts.autostart          = true;
+     238           9 :   shopts.queue_size         = 10;
+     239           9 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     240             : 
+     241           9 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diag_in");
+     242           9 :   sh_hw_api_status_        = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &AutomaticStart::callbackHwApiStatus, this);
+     243           9 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     244           9 :   sh_uav_manager_diag_     = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts, "uav_manager_diagnostics_in");
+     245          18 :   sh_gazebo_spawner_diag_  = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts, "gazebo_spawner_diagnostics_in",
+     246           9 :                                                                                           &AutomaticStart::callbackGazeboSpawnerDiagnostics, this);
+     247             : 
+     248             :   // | ----------------------- publishers ----------------------- |
+     249             : 
+     250           9 :   ph_can_takeoff_ = mrs_lib::PublisherHandler<std_msgs::Bool>(nh_, "can_takeoff_out");
+     251             : 
+     252             :   // | --------------------- service clients -------------------- |
+     253             : 
+     254           9 :   service_client_takeoff_               = nh_.serviceClient<std_srvs::Trigger>("takeoff_out");
+     255           9 :   service_client_eland_                 = nh_.serviceClient<std_srvs::Trigger>("eland_out");
+     256           9 :   service_client_toggle_control_output_ = nh_.serviceClient<std_srvs::SetBool>("toggle_control_output_out");
+     257           9 :   service_client_arm_                   = nh_.serviceClient<std_srvs::SetBool>("arm_out");
+     258             : 
+     259           9 :   service_client_validate_reference_ = nh_.serviceClient<mrs_msgs::ValidateReference>("validate_reference_out");
+     260             : 
+     261             :   // | ------------------ setup generic topics ------------------ |
+     262             : 
+     263           9 :   if (_topic_check_enabled_) {
+     264             : 
+     265          18 :     boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> callback;
+     266             : 
+     267          28 :     for (int i = 0; i < int(_topic_check_topic_names_.size()); i++) {
+     268             : 
+     269          38 :       std::string topic_name = _topic_check_topic_names_[i];
+     270             : 
+     271          19 :       if (topic_name.at(0) != '/') {
+     272          19 :         topic_name = "/" + _uav_name_ + "/" + topic_name;
+     273             :       }
+     274             : 
+     275          38 :       Topic tmp_topic(topic_name);
+     276          19 :       topic_check_topics_.push_back(tmp_topic);
+     277             : 
+     278          19 :       int id = i;  // id to identify which topic called the generic callback
+     279             : 
+     280        8976 :       callback                       = [this, topic_name, id](const topic_tools::ShapeShifter::ConstPtr& msg) -> void { genericCallback(msg, topic_name, id); };
+     281          57 :       ros::Subscriber tmp_subscriber = nh_.subscribe(topic_name, 1, callback);
+     282             : 
+     283          19 :       generic_subscriber_vec_.push_back(tmp_subscriber);
+     284             :     }
+     285             :   }
+     286             : 
+     287             :   // | ------------------------- timers ------------------------- |
+     288             : 
+     289           9 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &AutomaticStart::timerMain, this);
+     290             : 
+     291           9 :   is_initialized_ = true;
+     292             : 
+     293           9 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: initialized");
+     294           9 : }
+     295             : 
+     296             : //}
+     297             : 
+     298             : // --------------------------------------------------------------
+     299             : // |                          callbacks                         |
+     300             : // --------------------------------------------------------------
+     301             : 
+     302             : /* genericCallback() //{ */
+     303             : 
+     304        8956 : void AutomaticStart::genericCallback([[maybe_unused]] const topic_tools::ShapeShifter::ConstPtr& msg, [[maybe_unused]] const std::string& topic_name,
+     305             :                                      const int id) {
+     306        8956 :   topic_check_topics_[id].updateTime();
+     307        8957 : }
+     308             : 
+     309             : //}
+     310             : 
+     311             : /* callbackHwApiDiag() //{ */
+     312             : 
+     313       30156 : void AutomaticStart::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+     314             : 
+     315       30156 :   if (!is_initialized_) {
+     316           0 :     return;
+     317             :   }
+     318             : 
+     319       30156 :   ROS_INFO_ONCE("[AutomaticStart]: getting HW API diagnostics");
+     320             : 
+     321       60312 :   std::scoped_lock lock(mutex_hw_api_status_);
+     322             : 
+     323             :   // check armed_ state
+     324       30156 :   if (armed_ == false) {
+     325             : 
+     326             :     // if armed_ state changed to true, please "start the clock"
+     327       13219 :     if (msg->armed) {
+     328             : 
+     329           9 :       armed_      = true;
+     330           9 :       armed_time_ = ros::Time::now();
+     331             :     }
+     332             : 
+     333             :     // if we were armed_ previously
+     334       16937 :   } else if (armed_ == true) {
+     335             : 
+     336             :     // and we are not really now
+     337       16937 :     if (!msg->armed) {
+     338             : 
+     339           3 :       armed_ = false;
+     340             :     }
+     341             :   }
+     342             : 
+     343             :   // check offboard_ state
+     344       30156 :   if (offboard_ == false) {
+     345             : 
+     346             :     // if offboard_ state changed to true, please "start the clock"
+     347       15883 :     if (msg->offboard) {
+     348             : 
+     349           6 :       offboard_      = true;
+     350           6 :       offboard_time_ = ros::Time::now();
+     351             :     }
+     352             : 
+     353             :     // if we were in offboard_ previously
+     354       14273 :   } else if (offboard_ == true) {
+     355             : 
+     356             :     // and we are not really now
+     357       14273 :     if (!msg->offboard) {
+     358             : 
+     359           0 :       offboard_ = false;
+     360             :     }
+     361             :   }
+     362             : 
+     363       30156 :   if (msg->connected) {
+     364       29968 :     got_hw_api_status_ = true;
+     365             :   }
+     366             : }
+     367             : 
+     368             : //}
+     369             : 
+     370             : /* callbackGazeboSpawnerDiagnostics() //{ */
+     371             : 
+     372         232 : void AutomaticStart::callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg) {
+     373             : 
+     374         232 :   if (!is_initialized_) {
+     375           0 :     return;
+     376             :   }
+     377             : 
+     378         232 :   ROS_INFO_ONCE("[AutomaticStart]: getting spawner diagnostics");
+     379             : 
+     380             :   {
+     381         464 :     std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     382             : 
+     383         232 :     gazebo_spawner_diagnostics_ = *msg;
+     384             : 
+     385         232 :     got_gazebo_spawner_diagnostics = true;
+     386             :   }
+     387             : }
+     388             : 
+     389             : //}
+     390             : 
+     391             : // --------------------------------------------------------------
+     392             : // |                           timers                           |
+     393             : // --------------------------------------------------------------
+     394             : 
+     395             : /* timerMain() //{ */
+     396             : 
+     397        3866 : void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
+     398             : 
+     399        3866 :   if (!is_initialized_) {
+     400        1558 :     return;
+     401             :   }
+     402             : 
+     403        3866 :   bool got_uav_manager_diag     = sh_uav_manager_diag_.hasMsg();
+     404        3866 :   bool got_control_manager_diag = sh_control_manager_diag_.hasMsg();
+     405        3866 :   bool got_estimation_diag      = sh_estimation_diag_.hasMsg();
+     406             : 
+     407        3866 :   if (!got_control_manager_diag || !got_hw_api_status_ || !got_uav_manager_diag || !got_estimation_diag) {
+     408        1558 :     ROS_WARN_THROTTLE(5.0, "[AutomaticStart]: waiting for data: ControManager=%s, UavManager=%s, HW Api=%s, EstimationManager=%s",
+     409             :                       got_control_manager_diag ? "true" : "FALSE", got_uav_manager_diag ? "true" : "FALSE", got_hw_api_status_ ? "true" : "FALSE",
+     410             :                       got_estimation_diag ? "true" : "FALSE");
+     411        1558 :     return;
+     412             :   }
+     413             : 
+     414        2308 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     415        2308 :   auto control_manager_diagnostics                  = sh_control_manager_diag_.getMsg();
+     416             : 
+     417        2308 :   switch (current_state) {
+     418             : 
+     419        1567 :     case STATE_IDLE: {
+     420             : 
+     421        1567 :       bool   control_output   = sh_control_manager_diag_.getMsg()->output_enabled;
+     422        1567 :       double time_from_arming = (ros::Time::now() - armed_time).toSec();
+     423             : 
+     424        1567 :       std_msgs::Bool can_takeoff_msg;
+     425        1567 :       can_takeoff_msg.data = false;
+     426             : 
+     427             :       // | -------------------- preflight checks -------------------- |
+     428             : 
+     429        1567 :       bool position_valid = validateReference();
+     430        1567 :       bool got_topics     = topicCheck();
+     431        1567 :       bool speed_valid    = speedCheck();
+     432             : 
+     433        1567 :       bool can_takeoff = got_topics && position_valid && speed_valid;
+     434             : 
+     435             :       // | ---------------------------------------------------------- |
+     436             : 
+     437        1567 :       can_takeoff_msg.data = can_takeoff;
+     438        1567 :       ph_can_takeoff_.publish(can_takeoff_msg);
+     439             : 
+     440        1567 :       if (armed && !control_output) {
+     441             : 
+     442         145 :         if (can_takeoff) {
+     443             : 
+     444           6 :           bool res = toggleControlOutput(true);
+     445             : 
+     446           6 :           if (!res) {
+     447           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON");
+     448             :           }
+     449             :         }
+     450             : 
+     451         145 :         if (time_from_arming > _control_output_timeout_) {
+     452             : 
+     453           3 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON for %.2f secs, disarming", _control_output_timeout_);
+     454           3 :           disarm();
+     455           3 :           changeState(STATE_FINISHED);
+     456             :         }
+     457             :       }
+     458             : 
+     459        1567 :       if (_simulation_ && isGazeboSimulation()) {
+     460             : 
+     461         241 :         std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     462             : 
+     463         241 :         if (got_gazebo_spawner_diagnostics) {
+     464             : 
+     465         241 :           if (!gazebo_spawner_diagnostics_.spawn_called || gazebo_spawner_diagnostics_.processing) {
+     466           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) waiting for spawner to finish spawning UAVs");
+     467           0 :             return;
+     468             :           }
+     469             : 
+     470             :         } else {
+     471             : 
+     472           0 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) missing spawner diagnostics");
+     473           0 :           return;
+     474             :         }
+     475             :       }
+     476             : 
+     477             :       // when armed and in offboard, takeoff
+     478        1567 :       if (armed && offboard && control_output) {
+     479             : 
+     480         906 :         double armed_time_diff    = (ros::Time::now() - armed_time).toSec();
+     481         906 :         double offboard_time_diff = (ros::Time::now() - offboard_time).toSec();
+     482             : 
+     483         906 :         if ((armed_time_diff > _safety_timeout_) && (offboard_time_diff > _safety_timeout_)) {
+     484             : 
+     485           6 :           if (_handle_takeoff_) {
+     486           5 :             changeState(STATE_TAKEOFF);
+     487             :           } else {
+     488           1 :             changeState(STATE_FINISHED);
+     489             :           }
+     490             : 
+     491             :         } else {
+     492             : 
+     493         900 :           double min = (armed_time_diff < offboard_time_diff) ? armed_time_diff : offboard_time_diff;
+     494             : 
+     495         900 :           ROS_WARN_THROTTLE(1.0, "taking off in %.0f", (_safety_timeout_ - min));
+     496             :         }
+     497             :       }
+     498             : 
+     499        1567 :       break;
+     500             :     }
+     501             : 
+     502         732 :     case STATE_TAKEOFF: {
+     503             : 
+     504             :       // if takeoff finished
+     505         732 :       if (control_manager_diagnostics->flying_normally) {
+     506             : 
+     507           5 :         ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: takeoff finished");
+     508             : 
+     509           5 :         changeState(STATE_FINISHED);
+     510             : 
+     511             :       } else {
+     512             : 
+     513         727 :         ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: waiting for the takeoff to finish");
+     514             :       }
+     515             : 
+     516         732 :       break;
+     517             :     }
+     518             : 
+     519           9 :     case STATE_FINISHED: {
+     520             : 
+     521           9 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: finished");
+     522           9 :       ros::requestShutdown();
+     523           9 :       break;
+     524             :     }
+     525             :   }
+     526             : 
+     527             : }  // namespace automatic_start
+     528             : 
+     529             : //}
+     530             : 
+     531             : // --------------------------------------------------------------
+     532             : // |                          routines                          |
+     533             : // --------------------------------------------------------------
+     534             : 
+     535             : /* changeState() //{ */
+     536             : 
+     537          14 : void AutomaticStart::changeState(LandingStates_t new_state) {
+     538             : 
+     539          14 :   ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: switching states %s -> %s", state_names[current_state], state_names[new_state]);
+     540             : 
+     541          14 :   switch (new_state) {
+     542             : 
+     543           0 :     case STATE_IDLE: {
+     544             : 
+     545           0 :       break;
+     546             :     }
+     547             : 
+     548           5 :     case STATE_TAKEOFF: {
+     549             : 
+     550           5 :       if (_pre_takeoff_sleep_ > 1.0) {
+     551           0 :         ROS_INFO("[AutomaticStart]: sleeping for %.2f secs before takeoff", _pre_takeoff_sleep_);
+     552           0 :         ros::Duration(_pre_takeoff_sleep_).sleep();
+     553             :       }
+     554             : 
+     555           5 :       bool res = takeoff();
+     556             : 
+     557           5 :       if (!res) {
+     558           0 :         return;
+     559             :       }
+     560             : 
+     561           5 :       break;
+     562             :     }
+     563             : 
+     564           9 :     case STATE_FINISHED: {
+     565             : 
+     566           9 :       break;
+     567             :     }
+     568             : 
+     569             :     break;
+     570             :   }
+     571             : 
+     572          14 :   current_state = new_state;
+     573             : }
+     574             : 
+     575             : //}
+     576             : 
+     577             : /* takeoff() //{ */
+     578             : 
+     579           5 : bool AutomaticStart::takeoff() {
+     580             : 
+     581           5 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: taking off");
+     582             : 
+     583          10 :   std_srvs::Trigger srv;
+     584             : 
+     585           5 :   bool res = service_client_takeoff_.call(srv);
+     586             : 
+     587           5 :   if (res) {
+     588             : 
+     589           5 :     if (srv.response.success) {
+     590             : 
+     591           5 :       return true;
+     592             : 
+     593             :     } else {
+     594             : 
+     595           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: taking off failed: %s", srv.response.message.c_str());
+     596             :     }
+     597             : 
+     598             :   } else {
+     599             : 
+     600           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for taking off failed");
+     601             :   }
+     602             : 
+     603           0 :   return false;
+     604             : }
+     605             : 
+     606             : //}
+     607             : 
+     608             : /* validateReference() //{ */
+     609             : 
+     610        1567 : bool AutomaticStart::validateReference() {
+     611             : 
+     612        3134 :   mrs_msgs::ValidateReference srv_out;
+     613             : 
+     614        1567 :   srv_out.request.reference.header.frame_id = _body_frame_name_;
+     615             : 
+     616        1567 :   bool res = service_client_validate_reference_.call(srv_out);
+     617             : 
+     618        1567 :   if (res) {
+     619             : 
+     620        1567 :     if (srv_out.response.success) {
+     621             : 
+     622        1490 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: current position is valid");
+     623        1490 :       return true;
+     624             : 
+     625             :     } else {
+     626             : 
+     627          77 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position is not valid (safety area, bumper)!");
+     628          77 :       return false;
+     629             :     }
+     630             : 
+     631             :   } else {
+     632             : 
+     633           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position could not be validated");
+     634           0 :     return false;
+     635             :   }
+     636             : }
+     637             : 
+     638             : //}
+     639             : 
+     640             : /* toggleControlOutput() //{ */
+     641             : 
+     642           6 : bool AutomaticStart::toggleControlOutput(const bool& value) {
+     643             : 
+     644           6 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: setting control output %s", value ? "ON" : "OFF");
+     645             : 
+     646          12 :   std_srvs::SetBool srv;
+     647           6 :   srv.request.data = value;
+     648             : 
+     649           6 :   bool res = service_client_toggle_control_output_.call(srv);
+     650             : 
+     651           6 :   if (res) {
+     652             : 
+     653           6 :     if (srv.response.success) {
+     654             : 
+     655           6 :       return true;
+     656             : 
+     657             :     } else {
+     658             : 
+     659           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: setting of control output failed: %s", srv.response.message.c_str());
+     660             :     }
+     661             : 
+     662             :   } else {
+     663             : 
+     664           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for toggling control output failed");
+     665             :   }
+     666             : 
+     667           0 :   return false;
+     668             : }
+     669             : 
+     670             : //}
+     671             : 
+     672             : /* disarm() //{ */
+     673             : 
+     674           3 : bool AutomaticStart::disarm() {
+     675             : 
+     676           3 :   if (!got_hw_api_status_) {
+     677             : 
+     678           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, missing HW API status!");
+     679             : 
+     680           0 :     return false;
+     681             :   }
+     682             : 
+     683           3 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     684             : 
+     685           3 :   if (offboard) {
+     686             : 
+     687           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, already in offboard mode!");
+     688             : 
+     689           0 :     return false;
+     690             :   }
+     691             : 
+     692           3 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: disarming");
+     693             : 
+     694           6 :   std_srvs::SetBool srv;
+     695           3 :   srv.request.data = false;
+     696             : 
+     697           3 :   bool res = service_client_arm_.call(srv);
+     698             : 
+     699           3 :   if (res) {
+     700             : 
+     701           3 :     if (srv.response.success) {
+     702             : 
+     703           3 :       return true;
+     704             : 
+     705             :     } else {
+     706             : 
+     707           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: disarming failed");
+     708             :     }
+     709             : 
+     710             :   } else {
+     711             : 
+     712           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for disarming failed");
+     713             :   }
+     714             : 
+     715           0 :   return false;
+     716             : }
+     717             : 
+     718             : //}
+     719             : 
+     720             : /* isGazeboSimulation() //{ */
+     721             : 
+     722        1567 : bool AutomaticStart::isGazeboSimulation(void) {
+     723             : 
+     724        1567 :   if (is_gazebo_simulation_) {
+     725         240 :     return true;
+     726             :   }
+     727             : 
+     728        2654 :   ros::V_string node_list;
+     729        1327 :   ros::master::getNodes(node_list);
+     730             : 
+     731       18567 :   for (auto& node : node_list) {
+     732       17241 :     if (node.find("mrs_drone_spawner") != std::string::npos) {
+     733           1 :       ROS_INFO("[AutomaticStart]: MRS Gazebo Simulation detected");
+     734           1 :       is_gazebo_simulation_ = true;
+     735           1 :       return true;
+     736             :     }
+     737             :   }
+     738             : 
+     739        1326 :   return false;
+     740             : }
+     741             : 
+     742             : //}
+     743             : 
+     744             : /* topicCheck() //{ */
+     745             : 
+     746        1567 : bool AutomaticStart::topicCheck(void) {
+     747             : 
+     748        1567 :   bool got_topics = true;
+     749             : 
+     750        1567 :   std::stringstream missing_topics;
+     751             : 
+     752        1567 :   if (_topic_check_enabled_) {
+     753             : 
+     754        4777 :     for (int i = 0; i < int(topic_check_topics_.size()); i++) {
+     755        3210 :       if ((ros::Time::now() - topic_check_topics_[i].getTime()).toSec() > _topic_check_timeout_) {
+     756          76 :         missing_topics << std::endl << "\t" << topic_check_topics_[i].getTopicName();
+     757          76 :         got_topics = false;
+     758             :       }
+     759             :     }
+     760             :   }
+     761             : 
+     762        1567 :   if (!got_topics) {
+     763          76 :     ROS_WARN_STREAM_THROTTLE(1.0, "[AutomaticStart]: missing data on topics: " << missing_topics.str());
+     764             :   }
+     765             : 
+     766        3134 :   return got_topics;
+     767             : }
+     768             : 
+     769             : //}
+     770             : 
+     771             : /* velocityCheck() //{ */
+     772             : 
+     773        1567 : bool AutomaticStart::speedCheck(void) {
+     774             : 
+     775        1567 :   if (!_velocity_check_enabled_) {
+     776           0 :     return true;
+     777             :   }
+     778             : 
+     779        1567 :   if (!sh_estimation_diag_.hasMsg()) {
+     780           0 :     return false;
+     781             :   }
+     782             : 
+     783        3134 :   auto estimation_diag = sh_estimation_diag_.getMsg();
+     784             : 
+     785             :   double speed =
+     786        1567 :       sqrt(pow(estimation_diag->velocity.linear.x, 2.0) + pow(estimation_diag->velocity.linear.y, 2.0) + pow(estimation_diag->velocity.linear.z, 2.0));
+     787             : 
+     788        1567 :   if (speed > _velocity_check_max_speed_) {
+     789          73 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: the estimated speed (%.2f ms^-2) is over the limit (%.2f ms^-2)", speed, _velocity_check_max_speed_);
+     790          73 :     return false;
+     791             :   }
+     792             : 
+     793        1494 :   return true;
+     794             : }
+     795             : 
+     796             : //}
+     797             : 
+     798             : }  // namespace automatic_start
+     799             : 
+     800             : }  // namespace mrs_uav_autostart
+     801             : 
+     802             : #include <pluginlib/class_list_macros.h>
+     803           9 : 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..c52f45e149 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html @@ -0,0 +1,221 @@ + + + + + + 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 + + +
+ 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..1ad4a95611cf51a0144444214619a688390d984b GIT binary patch literal 2658 zcmV-o3Z3fB;{Y$NG3Yw(pNc z3%=<&qK_WQj3_`JT-w7XLL8yUqKgs5Am(w7BCnhQ#L1-_q)9}I0|Jp|lv%ouD6<|F zEfEy2kUJ@A>>8$c3Mr9RePy(Clll!yuMhvZYSDJ0YyxU>D|i3}brHAay1hbzGD7@0S9p~2;kjS+G&C5He5es2=IFj%&dU9ki2DzE6Cwi@D*hnrwFMP zuyqjmY`CJx<<})5NwUG?u)KjH)MFqExwxQ53YKL}-|;vO`iO-t6qfd|WyC9b+#pFE zJzjIeC6Aj9j}nLNI9}7rS(@_K&_?N8hk?}V7Twh~sWY?n)})_nj~gNrMezY$q=M*K z>JSUD+*hQIK>0~{xHbkc#hvQcG2#Z6e&E$FR|J4jtnLG)BeO?AaxL_F$VeiFp_$Z~h zB8R7#gxR1ZO7XgRa3TB63rM|vSIrXZui>VVvSpov-qPv97!rUjys}p6S?QJ2b~*q5 zG37Ab4rDIlwyUIrRaaNjM1^ax$SlQ9m3xrl_usZ(@I@7PzOp)6l-~EVAl@#V$BJ;L zx<92yQ+b-HUFowe%&51ob{*z*Jj0=}8SSDQeDBr1glizC53A z1YhUp*ZcMT;o<24AU>P0JYQ{ugeV8~%5bIdQ(Np_C%xH5%QsSrh(YIV>#r2uS?O^QmC>;T?Bi>6^-74cIaw{=G+0cLaz+iW59)&Bc-t zBkm(GK4NE_KQX1}B{KkW3qQ;#lr+vLynNA2-89?Nv+e>ZxEtPZfh4bc##YW^!uMd! z5r|TXahh}Moy|mR`#PH7aO8Y;a_oT*IvyT+CH zJTv)bmX2@e;Z-}MbzJuyHeULONAHn^ThC0~9_HAjGvohiwKQa~Oq`kSb05N`pT0=? zdUdL#--xmpcf*+x3XQJzO*@nKkZw5te<2UQ+N0)k+@qJ{ati<0lq)Ap<0WLV`Rq(` zR;CTpRW9}lJ57;2V*mQ#l3jAEIidS6xr$GIG7bRVa9vt*g9C78Ca|D;<^ZzTwyv%N zVT!q(N>dc#UQqM_&K+OJxk_S@kBFdx&6ivQL!f!^qQcG2}uVUAX9^qa$)lL7QgMB(8+FaNDalqBpY=%!~;7Gyq0 zy#d1t&s37ieb|hhz>L_5U=>BeNn}JxIMi1o>sNLT(}-WthblJa@Qw4yTR+ z!5)0MeH7x)U+=Gs*f|bkXGRR$rKj~vkO>_ffZ)=;2!(c1-%@wod}Sn+_Xw;VMg_~~ zbHhg=Qi6kaC~+tenWSrME`|1w;E>zw&7>41H@QHOJo5x1KaSF*$W?}SktZE9AKk+h zWdV`h7Mn7$*Jb1~q2&fG;HTqE=aG*njCOG!ucMYcfxZ&zuDmXPFCtV=6pT5#ok9z#yQtNZF$Xdbg zsWn#)=r0UZ=@px~W|JvAq{ft;3x}tZa6iq|_OvQ-XUgnT>aTv*EP>F;6(LGHxl+)N zt~&~vmQ4_ydeGBAFMl~Xp9eY>{Ca~r#hZdc87HDhKO^b^MI4n;j$Ks5$?E9}beGis zR?rk7-Y}OUe20V^1~1a_Yf3MlOcldj(N(cYT1GC`(i9?B(FU2I$Ug`;PO&UVlYVWm zr`an2j8Q~aE|vFHh{YK-9@Q^Mah40$>E0m>hI)M{*=c70>ar}nEa_R_Gs?ej7zBC- zopmV+R~}gUQwQ>?(>v$((xX<26hRspV2UE;_DX`k$nEitPo-GkXG6eqalhU_Vqq*2 zE7to>c#`C(VlFL=dbam>9cgOke!V@S!0*XD?xzj-={QLKfhD6|(1>5IpfNU%m#N4Q zJC9XpInp`x{LddX;MsB&A%FBK*T0cJhGh7E{@A4W5pQ}2C^j6ySa}xNOfsVwH@{wa z{7JHf`q5Pk@}by4h4<@^x(a`Pj>749eN=D2AXfPAir3Wpat%=YAFnmw*@$uek8~9N zjd=a~AFulq;h?)p)MiFFuNHg-0ecZ1!u`HC1?{yyh*Hz_qD#Ef*|vVFde4Do`wT=Z zOKfQ{49$ID#K!yHjQGbNoB5CxaXzYjrJ0G>=b8?jgQ-H>aKbjIsmH#fWEx>rTvHjh771Bku?!sO0`|Hh+#3)s20BQ)3v4jM(_ zK*_g?=DJsL7%)5tZ5Cm}=dMIkkm7!ZOVJ;*Ehs9GjF107AQ5S+t&I5i0}*nYn#T)3 Q5&!@I07*qoM6N<$f>>%4761SM 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..d9fe4f8b46 --- /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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
<unnamed>88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
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..7acde9d0d4 --- /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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
<unnamed>88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
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..b8523b11cd --- /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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
<unnamed>88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
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..e97dec1068 --- /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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
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..5333de5cb1 --- /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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
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..52d38d2d9b --- /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:23626888.1 %
Date:2023-12-06 07:59:45Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
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..54e170a4bd --- /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:2023-12-06 07:59:45Functions: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::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)1
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2
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&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<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::HwApiAttitudeRateCmd_<std::allocator<void> > const&)13
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)17
+
+
+ + + +
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..5fc196a168 --- /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:2023-12-06 07:59:45Functions: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&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)17
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&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)13
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<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::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)1
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..f4252cd6df --- /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:2023-12-06 07:59:45Functions: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           2 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      56             : 
+      57           2 :     double throttle = 0;
+      58             : 
+      59           2 :     if (msg.motors.size() == 0) {
+      60           0 :       return std::nullopt;
+      61             :     }
+      62             : 
+      63           2 :     throttle = 0;
+      64             : 
+      65          10 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      66           8 :       throttle += msg.motors[i];
+      67             :     };
+      68             : 
+      69           2 :     throttle /= msg.motors.size();
+      70             : 
+      71           2 :     return throttle;
+      72             :   }
+      73           2 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      74           2 :     return msg.throttle;
+      75             :   }
+      76          17 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      77          17 :     return msg.throttle;
+      78             :   }
+      79          13 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      80          13 :     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           1 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      89           1 :     return std::nullopt;
+      90             :   }
+      91           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      92           2 :     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:555894.8 %
Date:2023-12-06 07:59:45Functions: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 +
93.9%93.9%
+ 		93.9 %31 / 33100.0 %5 / 5
<unnamed>93.9 %31 / 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..720e9c5093 --- /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:555894.8 %
Date:2023-12-06 07:59:45Functions: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 +
93.9%93.9%
+ 		93.9 %31 / 33100.0 %5 / 5
<unnamed>93.9 %31 / 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.html b/mrs_uav_controllers/include/index-detail.html new file mode 100644 index 0000000000..de604e2f49 --- /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:555894.8 %
Date:2023-12-06 07:59:45Functions: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 +
93.9%93.9%
+ 		93.9 %31 / 33100.0 %5 / 5
<unnamed>93.9 %31 / 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..59f91c433a --- /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:555894.8 %
Date:2023-12-06 07:59:45Functions: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 +
93.9%93.9%
+ 		93.9 %31 / 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..54fd616c41 --- /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:555894.8 %
Date:2023-12-06 07:59:45Functions: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 +
93.9%93.9%
+ 		93.9 %31 / 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.html b/mrs_uav_controllers/include/index.html new file mode 100644 index 0000000000..9571faec4d --- /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:555894.8 %
Date:2023-12-06 07:59:45Functions: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 +
93.9%93.9%
+ 		93.9 %31 / 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..1ae4a0997c --- /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:313393.9 %
Date:2023-12-06 07:59:45Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::reset()504
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)504
mrs_uav_controllers::PIDController::PIDController()504
mrs_uav_controllers::PIDController::setSaturation(double)3092
mrs_uav_controllers::PIDController::update(double const&, double const&)3092
+
+
+ + + +
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..5453be1828 --- /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:313393.9 %
Date:2023-12-06 07:59:45Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::setSaturation(double)3092
mrs_uav_controllers::PIDController::reset()504
mrs_uav_controllers::PIDController::update(double const&, double const&)3092
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)504
mrs_uav_controllers::PIDController::PIDController()504
+
+
+ + + +
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..86df62a49c --- /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:313393.9 %
Date:2023-12-06 07:59:45Functions: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         504 : PIDController::PIDController() {
+      43             : 
+      44         504 :   this->reset();
+      45         504 : }
+      46             : 
+      47         504 : void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) {
+      48             : 
+      49         504 :   this->_kp_       = kp;
+      50         504 :   this->_kd_       = kd;
+      51         504 :   this->_ki_       = ki;
+      52         504 :   this->saturation = saturation;
+      53         504 :   this->antiwindup = antiwindup;
+      54         504 : }
+      55             : 
+      56        3092 : void PIDController::setSaturation(const double saturation) {
+      57             : 
+      58        3092 :   this->saturation = saturation;
+      59        3092 : }
+      60             : 
+      61         504 : void PIDController::reset(void) {
+      62             : 
+      63         504 :   this->last_error_ = 0;
+      64         504 :   this->integral_   = 0;
+      65         504 : }
+      66             : 
+      67        3092 : double PIDController::update(const double &error, const double &dt) {
+      68             : 
+      69             :   // calculate the control error difference
+      70        3092 :   double difference = (error - last_error_) / dt;
+      71        3092 :   last_error_       = error;
+      72             : 
+      73        3092 :   double p_component = _kp_ * error;       // proportional feedback
+      74        3092 :   double d_component = _kd_ * difference;  // derivative feedback
+      75        3092 :   double i_component = _ki_ * integral_;   // derivative feedback
+      76             : 
+      77        3092 :   double sum = p_component + d_component + i_component;
+      78             : 
+      79        3092 :   if (saturation > 0) {
+      80        3092 :     if (sum >= saturation) {
+      81           0 :       sum = saturation;
+      82        3092 :     } else if (sum <= -saturation) {
+      83           0 :       sum = -saturation;
+      84             :     }
+      85             :   }
+      86             : 
+      87        3092 :   if (antiwindup > 0) {
+      88        3092 :     if (std::abs(sum) < antiwindup) {
+      89             :       // add to the integral
+      90        2759 :       integral_ += error * dt;
+      91             :     }
+      92             :   }
+      93             : 
+      94             :   // return the summ of the components
+      95        3092 :   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..9667ce26842a9ead24557971d5cb3a0df01cd47f GIT binary patch literal 533 zcmV+w0_y#VP)#@dQ9wSy;%iQAIeL4O+=LtvN-1eSVrXn2+bp93 zwUSeXAft77SEfO(qhq;>+mSJm0O~TxRd^JUBjE-K)`1eit@FzQHgN^8jpLgF8FT?J zi@U-d8D+{`dnoiJBcK=o`7u)6)`r;8IG5B(Rp%pPM zol@s!@vAf2+{Z$pi7}lM6_Xb*`4`;BshnnH+h1;aYnW&~xC0~A5QuC>F3`rGr-Lpf z_nj|hld&tUy9vDpbm8i*o_ydx2u?~Dy)@rFzfT5R9v&ov$g4GO1K=B37d6zOhS#RS zNya#(AQ`sx$+VGg#H-+TT_zdjaksHMSmr9Dsn0O7YCIL77%D*fECLf;9k^A2b1~|Z zH2J|C$Y&&$W^)sxpO&|Md;=O|j(tCz?WSYGmrlp4!|FLm?-W*j4X?6FPKndJGaQ<=0 + + + + + + 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:2023-12-06 07:59:45Functions: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&)93
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)1586
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)2478
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&)5218
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)26231
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&)26231
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&)27923
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&)27923
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)30376
+
+
+ + + +
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..bd5550ef2a --- /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:2023-12-06 07:59:45Functions: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&)27923
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)2478
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)30376
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)93
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)1586
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)26231
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&)26231
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&)27923
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&)5218
+
+
+ + + +
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..6c54285fd2 --- /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:2023-12-06 07:59:45Functions: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       26231 : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd) {
+      12             : 
+      13             :   // orientation error
+      14       26231 :   Eigen::Matrix3d R_error = 0.5 * (Rd.transpose() * R - R.transpose() * Rd);
+      15             : 
+      16             :   // vectorize the orientation error
+      17             :   // clang-format off
+      18       26231 :     Eigen::Vector3d R_error_vec;
+      19       26231 :     R_error_vec << (R_error(1, 2) - R_error(2, 1)) / 2.0,
+      20       26231 :                    (R_error(2, 0) - R_error(0, 2)) / 2.0,
+      21       26231 :                    (R_error(0, 1) - R_error(1, 0)) / 2.0;
+      22             :   // clang-format on
+      23             : 
+      24       52462 :   return R_error_vec;
+      25             : }
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* sanitizeDesiredForce() //{ */
+      30             : 
+      31       27923 : 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       27923 :   double theta = acos(input[2]);
+      36       27923 :   double phi   = atan2(input[1], input[0]);
+      37             : 
+      38             :   // check for the failsafe limit
+      39       27923 :   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       27923 :   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       27923 :   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       27923 :   Eigen::Vector3d output(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
+      63             : 
+      64       27923 :   return {output};
+      65             : }
+      66             : 
+      67             : //}
+      68             : 
+      69             : /* so3transform() //{ */
+      70             : 
+      71       27923 : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
+      72             : 
+      73       27923 :   Eigen::Vector3d body_z_normed = body_z.normalized();
+      74             : 
+      75       27923 :   Eigen::Matrix3d Rd;
+      76             : 
+      77       27923 :   if (preserve_heading) {
+      78             : 
+      79        8009 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Baca's method");
+      80             : 
+      81             :     // | ------------------------- body z ------------------------- |
+      82        8009 :     Rd.col(2) = body_z_normed;
+      83             : 
+      84             :     // | ------------------------- body x ------------------------- |
+      85             : 
+      86             :     // construct the oblique projection
+      87        8009 :     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       16018 :     Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+      91        8009 :     A.col(0)          = projector_body_z_compl.col(0);
+      92        8009 :     A.col(1)          = projector_body_z_compl.col(1);
+      93             : 
+      94             :     // create the basis of the projection null-space complement
+      95       16018 :     Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+      96        8009 :     B.col(0)          = Eigen::Vector3d(1, 0, 0);
+      97        8009 :     B.col(1)          = Eigen::Vector3d(0, 1, 0);
+      98             : 
+      99             :     // oblique projector to <range_basis>
+     100       16018 :     Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     101       16018 :     Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     102        8009 :     Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     103             : 
+     104        8009 :     Rd.col(0) = oblique_projector * heading;
+     105        8009 :     Rd.col(0).normalize();
+     106             : 
+     107             :     // | ------------------------- body y ------------------------- |
+     108             : 
+     109        8009 :     Rd.col(1) = Rd.col(2).cross(Rd.col(0));
+     110        8009 :     Rd.col(1).normalize();
+     111             : 
+     112             :   } else {
+     113             : 
+     114       19914 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Lee's method");
+     115             : 
+     116       19914 :     Rd.col(2) = body_z_normed;
+     117       19914 :     Rd.col(1) = Rd.col(2).cross(heading);
+     118       19914 :     Rd.col(1).normalize();
+     119       19914 :     Rd.col(0) = Rd.col(1).cross(Rd.col(2));
+     120       19914 :     Rd.col(0).normalize();
+     121             :   }
+     122             : 
+     123       55846 :   return Rd;
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* getLowestOutput() //{ */
+     129             : 
+     130       30376 : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     131             : 
+     132       30376 :   if (outputs.actuators) {
+     133        2478 :     return ACTUATORS_CMD;
+     134             :   }
+     135             : 
+     136       27898 :   if (outputs.control_group) {
+     137        2740 :     return CONTROL_GROUP;
+     138             :   }
+     139             : 
+     140       25158 :   if (outputs.attitude_rate) {
+     141       21013 :     return ATTITUDE_RATE;
+     142             :   }
+     143             : 
+     144        4145 :   if (outputs.attitude) {
+     145        1692 :     return ATTITUDE;
+     146             :   }
+     147             : 
+     148        2453 :   if (outputs.acceleration_hdg_rate) {
+     149         687 :     return ACCELERATION_HDG_RATE;
+     150             :   }
+     151             : 
+     152        1766 :   if (outputs.acceleration_hdg) {
+     153         732 :     return ACCELERATION_HDG;
+     154             :   }
+     155             : 
+     156        1034 :   if (outputs.velocity_hdg_rate) {
+     157         341 :     return VELOCITY_HDG_RATE;
+     158             :   }
+     159             : 
+     160         693 :   if (outputs.velocity_hdg) {
+     161         347 :     return VELOCITY_HDG;
+     162             :   }
+     163             : 
+     164         346 :   if (outputs.position) {
+     165         346 :     return POSITION;
+     166             :   }
+     167             : 
+     168           0 :   return {};
+     169             : }
+     170             : 
+     171             : //}
+     172             : 
+     173             : /* getHighestOutput() //{ */
+     174             : 
+     175        1586 : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     176             : 
+     177        1586 :   if (outputs.position) {
+     178           2 :     return POSITION;
+     179             :   }
+     180             : 
+     181        1584 :   if (outputs.velocity_hdg) {
+     182           2 :     return VELOCITY_HDG;
+     183             :   }
+     184             : 
+     185        1582 :   if (outputs.velocity_hdg_rate) {
+     186           1 :     return VELOCITY_HDG_RATE;
+     187             :   }
+     188             : 
+     189        1581 :   if (outputs.acceleration_hdg) {
+     190           5 :     return ACCELERATION_HDG;
+     191             :   }
+     192             : 
+     193        1576 :   if (outputs.acceleration_hdg_rate) {
+     194           5 :     return ACCELERATION_HDG_RATE;
+     195             :   }
+     196             : 
+     197        1571 :   if (outputs.attitude) {
+     198        1498 :     return ATTITUDE;
+     199             :   }
+     200             : 
+     201          73 :   if (outputs.attitude_rate) {
+     202          13 :     return ATTITUDE_RATE;
+     203             :   }
+     204             : 
+     205          60 :   if (outputs.control_group) {
+     206          31 :     return CONTROL_GROUP;
+     207             :   }
+     208             : 
+     209          29 :   if (outputs.actuators) {
+     210          29 :     return ACTUATORS_CMD;
+     211             :   }
+     212             : 
+     213           0 :   return {};
+     214             : }
+     215             : 
+     216             : //}
+     217             : 
+     218             : /* extractThrottle() //{ */
+     219             : 
+     220          93 : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output) {
+     221             : 
+     222          93 :   if (!control_output.control_output) {
+     223          50 :     return {};
+     224             :   }
+     225             : 
+     226          86 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* attitudeController() //{ */
+     232             : 
+     233       26231 : 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       26231 :   Eigen::Matrix3d R  = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     238       26231 :   Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
+     239             : 
+     240             :   // calculate the orientation error
+     241       26231 :   Eigen::Vector3d E = common::orientationError(R, Rd);
+     242             : 
+     243       26231 :   Eigen::Vector3d rate_feedback = gains.array() * E.array() + ff_rate.array();
+     244             : 
+     245             :   // | ----------- parasitic heading rate compensation ---------- |
+     246             : 
+     247       26231 :   if (parasitic_heading_rate_compensation) {
+     248             : 
+     249        7150 :     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        7150 :     Eigen::Vector3d rp_heading_rate_compensation(0, 0, 0);
+     253             : 
+     254        7150 :     Eigen::Vector3d q_feedback_yawless = rate_feedback;
+     255        7150 :     q_feedback_yawless(2)              = 0;  // nullyfy the effect of the original yaw feedback
+     256             : 
+     257        7150 :     double parasitic_heading_rate = 0;
+     258             : 
+     259             :     try {
+     260        7150 :       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        7150 :       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        7150 :     rate_feedback += rp_heading_rate_compensation;
+     274             :   }
+     275             : 
+     276             :   // | --------------- saturate the attitude rate --------------- |
+     277             : 
+     278       26231 :   if (rate_feedback[0] > rate_saturation[0]) {
+     279           0 :     rate_feedback[0] = rate_saturation[0];
+     280       26231 :   } else if (rate_feedback[0] < -rate_saturation[0]) {
+     281           0 :     rate_feedback[0] = -rate_saturation[0];
+     282             :   }
+     283             : 
+     284       26231 :   if (rate_feedback[1] > rate_saturation[1]) {
+     285           0 :     rate_feedback[1] = rate_saturation[1];
+     286       26231 :   } else if (rate_feedback[1] < -rate_saturation[1]) {
+     287           0 :     rate_feedback[1] = -rate_saturation[1];
+     288             :   }
+     289             : 
+     290       26231 :   if (rate_feedback[2] > rate_saturation[2]) {
+     291           0 :     rate_feedback[2] = rate_saturation[2];
+     292       26231 :   } 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       26231 :   mrs_msgs::HwApiAttitudeRateCmd cmd;
+     299             : 
+     300       26231 :   cmd.stamp = ros::Time::now();
+     301             : 
+     302       26231 :   cmd.body_rate.x = rate_feedback[0];
+     303       26231 :   cmd.body_rate.y = rate_feedback[1];
+     304       26231 :   cmd.body_rate.z = rate_feedback[2];
+     305             : 
+     306       26231 :   cmd.throttle = reference.throttle;
+     307             : 
+     308       52462 :   return cmd;
+     309             : }
+     310             : 
+     311             : //}
+     312             : 
+     313             : /* attitudeRateController() //{ */
+     314             : 
+     315        5218 : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+     316             :                                                                      const Eigen::Vector3d& gains) {
+     317             : 
+     318        5218 :   Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
+     319        5218 :   Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     320             : 
+     321        5218 :   Eigen::Vector3d ctrl_group_action = (des_rate - cur_rate).array() * gains.array();
+     322             : 
+     323        5218 :   mrs_msgs::HwApiControlGroupCmd cmd;
+     324             : 
+     325        5218 :   cmd.stamp = ros::Time::now();
+     326             : 
+     327        5218 :   cmd.throttle = reference.throttle;
+     328             : 
+     329        5218 :   cmd.roll  = ctrl_group_action[0];
+     330        5218 :   cmd.pitch = ctrl_group_action[1];
+     331        5218 :   cmd.yaw   = ctrl_group_action[2];
+     332             : 
+     333       10436 :   return {cmd};
+     334             : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* actuatorMixer() //{ */
+     339             : 
+     340        2478 : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
+     341             : 
+     342        2478 :   Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
+     343             : 
+     344        4956 :   Eigen::VectorXd motors = mixer * ctrl_group;
+     345             : 
+     346        2478 :   double min = motors.minCoeff();
+     347             : 
+     348        2478 :   if (min < 0.0) {
+     349           0 :     motors.array() += abs(min);
+     350             :   }
+     351             : 
+     352        2478 :   double max = motors.maxCoeff();
+     353             : 
+     354        2478 :   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        2478 :   mrs_msgs::HwApiActuatorCmd actuator_msg;
+     373             : 
+     374        2478 :   actuator_msg.stamp = ros::Time::now();
+     375             : 
+     376       12390 :   for (int i = 0; i < motors.size(); i++) {
+     377        9912 :     actuator_msg.motors.push_back(motors[i]);
+     378             :   }
+     379             : 
+     380        4956 :   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:11319458.2 %
Date:2023-12-06 07:59:45Functions: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&)1
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)1
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()8
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()140
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)162
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)1396
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)39317
+
+
+ + + +
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..2c20239929 --- /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:11319458.2 %
Date:2023-12-06 07:59:45Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()8
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>)42
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)1396
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)162
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)39317
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)1
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&)1
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()140
+
+
+ + + +
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..53f347a37a --- /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:11319458.2 %
Date:2023-12-06 07:59:45Functions: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          42 : 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          42 :   nh_ = nh;
+     117             : 
+     118          42 :   common_handlers_  = common_handlers;
+     119          42 :   private_handlers_ = private_handlers;
+     120             : 
+     121          42 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123          42 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131          42 :   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          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141          84 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153          42 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158          42 :   _descend_speed_        = std::abs(_descend_speed_);
+     159          42 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161          42 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165          42 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166          42 :   shopts.nh                 = nh_;
+     167          42 :   shopts.node_name          = "FailsafeController";
+     168          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169          42 :   shopts.threadsafe         = true;
+     170          42 :   shopts.autostart          = true;
+     171          42 :   shopts.queue_size         = 10;
+     172          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174          42 :   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          42 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182          42 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186          42 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188          42 :   is_initialized_ = true;
+     189             : 
+     190          42 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           1 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199           2 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           1 :   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           1 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213           2 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           1 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           1 :       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           1 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           1 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           1 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           0 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           1 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           1 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           1 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           1 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           1 :   first_iteration_ = true;
+     243             : 
+     244           1 :   is_active_ = true;
+     245             : 
+     246           1 :   return true;
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* deactivate() //{ */
+     252             : 
+     253           8 : void FailsafeController::deactivate(void) {
+     254             : 
+     255           8 :   is_active_           = false;
+     256           8 :   first_iteration_     = false;
+     257           8 :   uav_mass_difference_ = 0;
+     258             : 
+     259           8 :   ROS_INFO("[FailsafeController]: deactivated");
+     260           8 : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* updateInactive() //{ */
+     265             : 
+     266       39317 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268       39317 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270       39317 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272       39317 :   first_iteration_ = false;
+     273       39317 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279        1396 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282        4188 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283        4188 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286        2792 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288        1396 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291        1396 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295        1396 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301        1396 :   if (first_iteration_) {
+     302           1 :     dt               = 0.01;
+     303           1 :     first_iteration_ = false;
+     304             :   } else {
+     305        1395 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308        1396 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310        1396 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312        1396 :   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        1396 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317        1396 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323        2792 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325        1396 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327        1396 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329        1396 :   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        1396 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340        1396 :   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        1396 :   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        1396 :   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        1396 :   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        1396 :   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        1396 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423        1396 :   attitude_cmd.stamp       = ros::Time::now();
+     424        1396 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425        1396 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427        1396 :   if (highest_modality.value() == common::ATTITUDE) {
+     428        1396 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude output");
+     429        1396 :     control_output.control_output = attitude_cmd;
+     430        1396 :     return control_output;
+     431             :   }
+     432             : 
+     433             :   // --------------------------------------------------------------
+     434             :   // |                      attitude control                      |
+     435             :   // --------------------------------------------------------------
+     436             : 
+     437           0 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438           0 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439           0 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441           0 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443           0 :   if (highest_modality.value() == common::ATTITUDE_RATE) {
+     444           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude rate output");
+     445           0 :     control_output.control_output = attitude_rate_command;
+     446           0 :     return control_output;
+     447             :   }
+     448             : 
+     449             :   // --------------------------------------------------------------
+     450             :   // |                    attitude rate control                   |
+     451             :   // --------------------------------------------------------------
+     452             : 
+     453           0 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455           0 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457           0 :   if (highest_modality.value() == common::CONTROL_GROUP) {
+     458           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning control group output");
+     459           0 :     control_output.control_output = control_group_command;
+     460           0 :     return control_output;
+     461             :   }
+     462             : 
+     463             :   // --------------------------------------------------------------
+     464             :   // |                            mixer                           |
+     465             :   // --------------------------------------------------------------
+     466             : 
+     467           0 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469           0 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470           0 :   control_output.control_output = actuator_cmd;
+     471           0 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478         140 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480         140 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482         140 :   controller_status.active = is_active_;
+     483             : 
+     484         140 :   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         162 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         162 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         162 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         162 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         324 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         162 :   res.success = true;
+     518         162 :   res.message = "constraints updated";
+     519             : 
+     520         162 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           1 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           1 :     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          42 : 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..a3b7b80beaf03c128f862879ce8f0716d0bbb028 GIT binary patch literal 1823 zcmV+)2jKXLP)(T(Ly6qkY@i3>qN z*H_J2=2}o33JTIp#eVGN!yCAyA27wm9hDGU3i7)Kc67BQoJOnc2%JJF>pyOm$`L7) zr39dbd^C(s2 zx6R}ct}Kz5Ox?{{m_7Y~$#W-3^;a(SANtW>wYN2=E zS0vaBLC}9k1p@_rdNHC@$Ae9s87+c;}I>l8O`nXXHMPKi}kRT~FAU4G_t32oYVTa7B!A6K8MhT`?hnjwy}6^V(6zIKfjA#Jp-#EVwLt z)Yjv_b>BY!Ok0j-1V5i`3*EZqO2P^98%*}SbKRZtx@hpI2?PbcHsR-jaLRkC0ed2J zfnfWt-+=f)*QEq;LI%M%M4>a!k%=&8bv){<*=&P?6)v16V)B&F3br+<9~1UWb73w- zCey`P4ovJ;FmLaO;Ti}Q5w%4u*fQ<jxhI_mS=XYip0{IZ z{F0?qsg#O-rxDqL*{*$;_9Zy!jKpqa&g=p^LPF3watFhK6C?hzy65q|=8P$%Esr7D!6mrSI2cy&iLyY^`uBsSnfB3hVJhQ>ZqgkBnw}Rn?d-i@K zZZG<>3X#~^s}?M)2n4q!2xgG$%oArbNB!e*>Y0LTzMJ-nd_-CO#u-w9k^c!F7m8=K zf~1aUFw{1;d@vLr*H`edE{BIZzTw$dayZK3zcyyB&S&Zb;phv3$8#c!w~d+OgIB~1 zox?*Gzr)8oBmX)dca!Z)kbjhq@2JC3gqR<>P+-SQKpf$XQ%Z17H!DH#LO~r6)BTj{ zt)}vF4;>=Id@{KgbE=%2HoG-*tfdijy^}gN-vq4WnHe_^K)n| z*<%F0!k+cFv&S$9)8Kjb04rl#*06{6pm|xkf(J8e1)c|&j_E;x3m?1lq1qhnA1yx6 zkI$&&i;8T?m8oYc5p98#7nC5DH3Nb#x*cXhxXE3heJ3Y_)mH%Qm|@B-&;K!b0}D~+ z*?>vjWuAuD$jaxAFuj1=v&2`l?8XrDngp8Ib8hrA*H4^4Mrn6)<`HDG$YdU|?y#w# zV + + + + + + 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:1712217278.8 %
Date:2023-12-06 07:59:45Functions:576686.4 %
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 +
58.2%58.2%
+ 		58.2 %113 / 19481.8 %9 / 11
<unnamed>58.2 %113 / 19481.8 %9 / 11
mpc_controller.cpp +
80.8%80.8%
+ 		80.8 %722 / 89483.3 %15 / 18
<unnamed>80.8 %722 / 89483.3 %15 / 18
se3_controller.cpp +
78.7%78.7%
+ 		78.7 %609 / 77488.2 %15 / 17
<unnamed>78.7 %609 / 77488.2 %15 / 17
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..5f9c4f5d70 --- /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:1712217278.8 %
Date:2023-12-06 07:59:45Functions:576686.4 %
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 +
58.2%58.2%
+ 		58.2 %113 / 19481.8 %9 / 11
<unnamed>58.2 %113 / 19481.8 %9 / 11
se3_controller.cpp +
78.7%78.7%
+ 		78.7 %609 / 77488.2 %15 / 17
<unnamed>78.7 %609 / 77488.2 %15 / 17
mpc_controller.cpp +
80.8%80.8%
+ 		80.8 %722 / 89483.3 %15 / 18
<unnamed>80.8 %722 / 89483.3 %15 / 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..cf5db34f24 --- /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:1712217278.8 %
Date:2023-12-06 07:59:45Functions:576686.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.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
58.2%58.2%
+ 		58.2 %113 / 19481.8 %9 / 11
<unnamed>58.2 %113 / 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 +
80.8%80.8%
+ 		80.8 %722 / 89483.3 %15 / 18
<unnamed>80.8 %722 / 89483.3 %15 / 18
se3_controller.cpp +
78.7%78.7%
+ 		78.7 %609 / 77488.2 %15 / 17
<unnamed>78.7 %609 / 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..5a80d1a7b9 --- /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:1712217278.8 %
Date:2023-12-06 07:59:45Functions:576686.4 %
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 +
58.2%58.2%
+ 		58.2 %113 / 19481.8 %9 / 11
mpc_controller.cpp +
80.8%80.8%
+ 		80.8 %722 / 89483.3 %15 / 18
se3_controller.cpp +
78.7%78.7%
+ 		78.7 %609 / 77488.2 %15 / 17
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..30cea43084 --- /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:1712217278.8 %
Date:2023-12-06 07:59:45Functions:576686.4 %
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 +
58.2%58.2%
+ 		58.2 %113 / 19481.8 %9 / 11
se3_controller.cpp +
78.7%78.7%
+ 		78.7 %609 / 77488.2 %15 / 17
mpc_controller.cpp +
80.8%80.8%
+ 		80.8 %722 / 89483.3 %15 / 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..4238638812 --- /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:1712217278.8 %
Date:2023-12-06 07:59:45Functions:576686.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.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
58.2%58.2%
+ 		58.2 %113 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
80.8%80.8%
+ 		80.8 %722 / 89483.3 %15 / 18
se3_controller.cpp +
78.7%78.7%
+ 		78.7 %609 / 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..168d232935 --- /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:2023-12-06 07:59:45Functions: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&)9
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()39
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()52
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)66
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)162
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)190
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)40523
+
+
+ + + +
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..2685db6ad3 --- /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:2023-12-06 07:59:45Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
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
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_controllers::mpc_controller::MpcController::deactivate()62
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)80
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>)84
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)84
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)181
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)324
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&)330
mrs_uav_controllers::mpc_controller::MpcController::getStatus()3970
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)4828
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&)20609
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)20939
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)21120
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)38624
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)60306
+
+
+ + + +
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..2072ce4be9 --- /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:72289480.8 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_controllers::mpc_controller::MpcController::deactivate()62
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>)84
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)4828
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)84
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)21120
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)324
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)60306
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)20939
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&)330
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)38624
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)181
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
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&)20609
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)80
mrs_uav_controllers::mpc_controller::MpcController::getStatus()3970
+
+
+ + + +
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..013b499856 --- /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:72289480.8 %
Date:2023-12-06 07:59:45Functions:151883.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 <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          84 : 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          84 :   nh_ = nh;
+     260             : 
+     261          84 :   common_handlers_  = common_handlers;
+     262          84 :   private_handlers_ = private_handlers;
+     263             : 
+     264          84 :   _uav_mass_ = common_handlers_->getMass();
+     265          84 :   name_      = private_handlers_->runtime_name;
+     266             : 
+     267          84 :   ros::Time::waitForValid();
+     268             : 
+     269             :   // | ---------- loading params using the parent's nh ---------- |
+     270             : 
+     271         168 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     272             : 
+     273          84 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     274             : 
+     275          84 :   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          84 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/mpc_controller.yaml");
+     283          84 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/mpc_controller.yaml");
+     284          84 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/" + private_handlers->name_space + ".yaml");
+     285          84 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/" + private_handlers->name_space + ".yaml");
+     286             : 
+     287         168 :   const std::string yaml_namespace = "mrs_uav_controllers/" + private_handlers_->name_space + "/";
+     288             : 
+     289             :   // load the dynamicall model parameters
+     290          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/number_of_states", _n_states_);
+     291          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt1", _dt1_);
+     292          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt2", _dt2_);
+     293             : 
+     294          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizon_length", _horizon_length_);
+     295             : 
+     296          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_speed", _max_speed_horizontal_);
+     297          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_acceleration", _max_acceleration_horizontal_);
+     298          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_jerk", _max_jerk_);
+     299             : 
+     300          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/Q", _mat_Q_);
+     301          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/S", _mat_S_);
+     302             : 
+     303          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_speed", _max_speed_vertical_);
+     304          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_acceleration", _max_acceleration_vertical_);
+     305          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_u", _max_u_vertical_);
+     306             : 
+     307          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/Q", _mat_Q_z_);
+     308          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/S", _mat_S_z_);
+     309             : 
+     310          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/verbose", _mpc_solver_verbose_);
+     311          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/max_iterations", _mpc_solver_max_iterations_);
+     312             : 
+     313             :   // | ------------------------- rampup ------------------------- |
+     314             : 
+     315          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/enabled", _rampup_enabled_);
+     316          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/speed", _rampup_speed_);
+     317             : 
+     318             :   // | --------------------- integral gains --------------------- |
+     319             : 
+     320          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw", gains_.kiwxy);
+     321          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kib", gains_.kibxy);
+     322             : 
+     323             :   // integrator limits
+     324          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw_lim", gains_.kiwxy_lim);
+     325          84 :   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          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/roll_pitch", gains_.kq_roll_pitch);
+     331          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/yaw", gains_.kq_yaw);
+     332             : 
+     333             :   // attitude rate gains
+     334          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/roll_pitch", gains_.kw_rp);
+     335          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/yaw", gains_.kw_y);
+     336             : 
+     337             :   // mass estimator
+     338          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km", gains_.km);
+     339          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km_lim", gains_.km_lim);
+     340             : 
+     341             :   // constraints
+     342          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     343          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     344             : 
+     345          84 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     346             : 
+     347          84 :   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          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/throttle_saturation", _throttle_saturation_);
+     353             : 
+     354             :   // gain filtering
+     355          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     356          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     357          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/rate", _gain_filtering_rate_);
+     358          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     359             : 
+     360             :   // angular rate feed forward
+     361          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     362             : 
+     363             :   // output mode
+     364          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/preferred_output", drs_params_.preferred_output_mode);
+     365             : 
+     366             :   // | ------------------- position pid params ------------------ |
+     367             : 
+     368          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     369          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     370          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     371             : 
+     372          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     373          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     374          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     375             : 
+     376          84 :   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          84 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     384          84 :         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          84 :   uav_mass_difference_ = 0;
+     390          84 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     391          84 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     392             : 
+     393             :   // | ----------------- prepare the MPC solver ----------------- |
+     394             : 
+     395          84 :   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          84 :                                                                             _dt2_, 0, 1.0);
+     397          84 :   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          84 :                                                                             _dt2_, 0, 1.0);
+     399          84 :   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          84 :                                                                             _dt1_, _dt2_, 0.5, 0.5);
+     401             : 
+     402             :   // | --------------- dynamic reconfigure server --------------- |
+     403             : 
+     404          84 :   drs_params_.kiwxy         = gains_.kiwxy;
+     405          84 :   drs_params_.kibxy         = gains_.kibxy;
+     406          84 :   drs_params_.kq_roll_pitch = gains_.kq_roll_pitch;
+     407          84 :   drs_params_.kq_yaw        = gains_.kq_yaw;
+     408          84 :   drs_params_.km            = gains_.km;
+     409          84 :   drs_params_.km_lim        = gains_.km_lim;
+     410          84 :   drs_params_.kiwxy_lim     = gains_.kiwxy_lim;
+     411          84 :   drs_params_.kibxy_lim     = gains_.kibxy_lim;
+     412             : 
+     413          84 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     414          84 :   drs_->updateConfig(drs_params_);
+     415          84 :   Drs_t::CallbackType f = boost::bind(&MpcController::callbackDrs, this, _1, _2);
+     416          84 :   drs_->setCallback(f);
+     417             : 
+     418             :   // | --------------------- service servers -------------------- |
+     419             : 
+     420          84 :   service_set_integral_terms_ = nh_.advertiseService("set_integral_terms_in", &MpcController::callbackSetIntegralTerms, this);
+     421             : 
+     422             :   // | ------------------------- timers ------------------------- |
+     423             : 
+     424          84 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &MpcController::timerGains, this, false, false);
+     425             : 
+     426             :   // | ---------------------- position pid ---------------------- |
+     427             : 
+     428          84 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     429          84 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     430          84 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     431          84 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     432             : 
+     433             :   // | ------------------------ profiler ------------------------ |
+     434             : 
+     435          84 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MpcController", _profiler_enabled_);
+     436             : 
+     437             :   // | ----------------------- finish init ---------------------- |
+     438             : 
+     439          84 :   ROS_INFO("[%s]: initialized", this->name_.c_str());
+     440             : 
+     441          84 :   is_initialized_ = true;
+     442             : 
+     443          84 :   return true;
+     444             : }
+     445             : 
+     446             : //}
+     447             : 
+     448             : /* //{ activate() */
+     449             : 
+     450          80 : bool MpcController::activate(const ControlOutput &last_control_output) {
+     451             : 
+     452          80 :   activation_control_output_ = last_control_output;
+     453             : 
+     454          80 :   double activation_mass = _uav_mass_;
+     455             : 
+     456          80 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     457           3 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     458           3 :     activation_mass += uav_mass_difference_;
+     459           3 :     ROS_INFO("[%s]: setting mass difference from the last control output: %.2f kg", this->name_.c_str(), uav_mass_difference_);
+     460             :   }
+     461             : 
+     462          80 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     463             : 
+     464          80 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     465           3 :     Ib_b_[0] = -activation_control_output_.diagnostics.disturbance_bx_b;
+     466           3 :     Ib_b_[1] = -activation_control_output_.diagnostics.disturbance_by_b;
+     467             : 
+     468           3 :     Iw_w_[0] = -activation_control_output_.diagnostics.disturbance_wx_w;
+     469           3 :     Iw_w_[1] = -activation_control_output_.diagnostics.disturbance_wy_w;
+     470             : 
+     471           3 :     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          80 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     479             : 
+     480             :   // rampup check
+     481          80 :   if (_rampup_enabled_ && throttle_last_controller) {
+     482             : 
+     483          26 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     484             : 
+     485          26 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     486             : 
+     487          26 :     if (throttle_difference > 0) {
+     488          12 :       rampup_direction_ = 1;
+     489          14 :     } else if (throttle_difference < 0) {
+     490           1 :       rampup_direction_ = -1;
+     491             :     } else {
+     492          13 :       rampup_direction_ = 0;
+     493             :     }
+     494             : 
+     495          26 :     ROS_INFO("[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
+     496             : 
+     497          26 :     rampup_active_     = true;
+     498          26 :     rampup_start_time_ = ros::Time::now();
+     499          26 :     rampup_last_time_  = ros::Time::now();
+     500          26 :     rampup_throttle_   = throttle_last_controller.value();
+     501             : 
+     502          26 :     rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
+     503             :   }
+     504             : 
+     505          80 :   first_iteration_ = true;
+     506          80 :   mute_gains_      = true;
+     507             : 
+     508          80 :   timer_gains_.start();
+     509             : 
+     510          80 :   ROS_INFO("[%s]: activated", this->name_.c_str());
+     511             : 
+     512          80 :   is_active_ = true;
+     513             : 
+     514          80 :   return true;
+     515             : }
+     516             : 
+     517             : //}
+     518             : 
+     519             : /* //{ deactivate() */
+     520             : 
+     521          62 : void MpcController::deactivate(void) {
+     522             : 
+     523          62 :   is_active_           = false;
+     524          62 :   first_iteration_     = false;
+     525          62 :   uav_mass_difference_ = 0;
+     526             : 
+     527          62 :   timer_gains_.stop();
+     528             : 
+     529          62 :   ROS_INFO("[%s]: deactivated", this->name_.c_str());
+     530          62 : }
+     531             : 
+     532             : //}
+     533             : 
+     534             : /* updateInactive() //{ */
+     535             : 
+     536       60306 : void MpcController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     537             : 
+     538       60306 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     539             : 
+     540       60306 :   last_update_time_ = uav_state.header.stamp;
+     541             : 
+     542       60306 :   first_iteration_ = false;
+     543       60306 : }
+     544             : 
+     545             : //}
+     546             : 
+     547             : /* //{ updateWhenAcctive() */
+     548             : 
+     549       21120 : MpcController::ControlOutput MpcController::updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     550             : 
+     551       63360 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     552       63360 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     553             : 
+     554       42240 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     555             : 
+     556       21120 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     557             : 
+     558       21120 :   last_control_output_.desired_heading_rate          = {};
+     559       21120 :   last_control_output_.desired_orientation           = {};
+     560       21120 :   last_control_output_.desired_unbiased_acceleration = {};
+     561       21120 :   last_control_output_.control_output                = {};
+     562             : 
+     563       21120 :   if (!is_active_) {
+     564           0 :     return last_control_output_;
+     565             :   }
+     566             : 
+     567             :   // | -------------------- calculate the dt -------------------- |
+     568             : 
+     569             :   double dt;
+     570             : 
+     571       21120 :   if (first_iteration_) {
+     572          29 :     dt               = 0.01;
+     573          29 :     first_iteration_ = false;
+     574             :   } else {
+     575       21091 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     576             :   }
+     577             : 
+     578       21120 :   last_update_time_ = uav_state.header.stamp;
+     579             : 
+     580       21120 :   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       21120 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     590             : 
+     591       21120 :   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       21120 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     601       16809 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing attitude rate output", name_.c_str());
+     602       16809 :     lowest_modality = common::ATTITUDE_RATE;
+     603        4311 :   } 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        4311 :   } 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        4311 :   } 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       21120 :   switch (lowest_modality.value()) {
+     615             : 
+     616         181 :     case common::POSITION: {
+     617         181 :       positionPassthrough(uav_state, tracker_command);
+     618         181 :       break;
+     619             :     }
+     620             : 
+     621         169 :     case common::VELOCITY_HDG: {
+     622         169 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     623         169 :       break;
+     624             :     }
+     625             : 
+     626         161 :     case common::VELOCITY_HDG_RATE: {
+     627         161 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     628         161 :       break;
+     629             :     }
+     630             : 
+     631         369 :     case common::ACCELERATION_HDG: {
+     632         369 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     633         369 :       break;
+     634             :     }
+     635             : 
+     636         326 :     case common::ACCELERATION_HDG_RATE: {
+     637         326 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     638         326 :       break;
+     639             :     }
+     640             : 
+     641         833 :     case common::ATTITUDE: {
+     642         833 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE);
+     643         833 :       break;
+     644             :     }
+     645             : 
+     646       16809 :     case common::ATTITUDE_RATE: {
+     647       16809 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     648       16809 :       break;
+     649             :     }
+     650             : 
+     651        1208 :     case common::CONTROL_GROUP: {
+     652        1208 :       MPC(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     653        1208 :       break;
+     654             :     }
+     655             : 
+     656        1064 :     case common::ACTUATORS_CMD: {
+     657        1064 :       MPC(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     658        1064 :       break;
+     659             :     }
+     660             : 
+     661           0 :     default: {
+     662           0 :       break;
+     663             :     }
+     664             :   }
+     665             : 
+     666       21120 :   return last_control_output_;
+     667             : }
+     668             : 
+     669             : //}
+     670             : 
+     671             : /* //{ getStatus() */
+     672             : 
+     673        3970 : const mrs_msgs::ControllerStatus MpcController::getStatus() {
+     674             : 
+     675        3970 :   mrs_msgs::ControllerStatus controller_status;
+     676             : 
+     677        3970 :   controller_status.active = is_active_;
+     678             : 
+     679        3970 :   return controller_status;
+     680             : }
+     681             : 
+     682             : //}
+     683             : 
+     684             : /* switchOdometrySource() //{ */
+     685             : 
+     686           0 : void MpcController::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     687             : 
+     688           0 :   ROS_INFO("[%s]: switching the odometry source", this->name_.c_str());
+     689             : 
+     690           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     691             : 
+     692             :   // | ----- transform world disturabances to the new frame ----- |
+     693             : 
+     694           0 :   geometry_msgs::Vector3Stamped world_integrals;
+     695             : 
+     696           0 :   world_integrals.header.stamp    = ros::Time::now();
+     697           0 :   world_integrals.header.frame_id = uav_state.header.frame_id;
+     698             : 
+     699           0 :   world_integrals.vector.x = Iw_w_[0];
+     700           0 :   world_integrals.vector.y = Iw_w_[1];
+     701           0 :   world_integrals.vector.z = 0;
+     702             : 
+     703           0 :   auto res = common_handlers_->transformer->transformSingle(world_integrals, new_uav_state.header.frame_id);
+     704             : 
+     705           0 :   if (res) {
+     706             : 
+     707           0 :     std::scoped_lock lock(mutex_integrals_);
+     708             : 
+     709           0 :     Iw_w_[0] = res.value().vector.x;
+     710           0 :     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           0 : }
+     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         324 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcController::setConstraints([
+     740             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     741             : 
+     742         324 :   if (!is_initialized_) {
+     743           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     744             :   }
+     745             : 
+     746         324 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     747             : 
+     748         324 :   ROS_INFO("[%s]: updating constraints", this->name_.c_str());
+     749             : 
+     750         648 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     751         324 :   res.success = true;
+     752         324 :   res.message = "constraints updated";
+     753             : 
+     754         324 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     755             : }
+     756             : 
+     757             : //}
+     758             : 
+     759             : // --------------------------------------------------------------
+     760             : // |                         controllers                        |
+     761             : // --------------------------------------------------------------
+     762             : 
+     763             : /* Mpc() //{ */
+     764             : 
+     765       20609 : 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       41218 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     769       20609 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     770       20609 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     771             : 
+     772             :   // | ----------------- get the current heading ---------------- |
+     773             : 
+     774       20609 :   double uav_heading = getHeadingSafely(uav_state, tracker_command);
+     775             : 
+     776             :   // | ------------------- prepare constraints ------------------ |
+     777             : 
+     778       20609 :   double max_speed_horizontal        = _max_speed_horizontal_;
+     779       20609 :   double max_speed_vertical          = _max_speed_vertical_;
+     780       20609 :   double max_acceleration_horizontal = _max_acceleration_horizontal_;
+     781       20609 :   double max_acceleration_vertical   = _max_acceleration_vertical_;
+     782       20609 :   double max_jerk                    = _max_jerk_;
+     783       20609 :   double max_u_vertical              = _max_u_vertical_;
+     784             : 
+     785       20609 :   if (tracker_command.use_full_state_prediction) {
+     786             : 
+     787       11967 :     max_speed_horizontal = 1.5 * (constraints.horizontal_speed);
+     788       11967 :     max_speed_vertical   = 1.5 * (constraints.vertical_ascending_speed < constraints.vertical_descending_speed ? constraints.vertical_ascending_speed
+     789             :                                                                                                              : constraints.vertical_descending_speed);
+     790             : 
+     791       11967 :     max_acceleration_horizontal = 1.5 * (constraints.horizontal_acceleration);
+     792       11967 :     max_acceleration_vertical =
+     793       11967 :         1.5 * (constraints.vertical_ascending_acceleration < constraints.vertical_descending_acceleration ? constraints.vertical_ascending_acceleration
+     794             :                                                                                                           : constraints.vertical_descending_acceleration);
+     795             : 
+     796       11967 :     max_jerk       = 1.5 * constraints.horizontal_jerk;
+     797       11967 :     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       20609 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     810       20609 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     811       20609 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     812             : 
+     813       20609 :   Rp << tracker_command.position.x, tracker_command.position.y, tracker_command.position.z;  // fill the desired position
+     814       20609 :   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       20609 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     821       20609 :   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       20609 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     825             : 
+     826             :   // Ow - UAV angular rate
+     827       20609 :   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       20609 :   Eigen::Vector3d Ep = Rp - Op;
+     836       20609 :   Eigen::Vector3d Ev = Rv - Ov;
+     837             : 
+     838             :   // | ------------------- initial conditions ------------------- |
+     839             : 
+     840       41218 :   Eigen::MatrixXd initial_x = Eigen::MatrixXd::Zero(3, 1);
+     841       41218 :   Eigen::MatrixXd initial_y = Eigen::MatrixXd::Zero(3, 1);
+     842       41218 :   Eigen::MatrixXd initial_z = Eigen::MatrixXd::Zero(3, 1);
+     843             : 
+     844             :   /* initial x //{ */
+     845             : 
+     846             :   {
+     847             :     double acceleration;
+     848             :     double velocity;
+     849       20609 :     double coef = 1.5;
+     850             : 
+     851       20609 :     if (fabs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
+     852       20609 :       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       20609 :     if (fabs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
+     861       20609 :       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       20609 :     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       20609 :     double coef = 1.5;
+     880             : 
+     881       20609 :     if (fabs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
+     882       20609 :       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       20609 :     if (fabs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
+     891       20609 :       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       20609 :     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       20609 :     double coef = 1.5;
+     910             : 
+     911       20609 :     if (fabs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
+     912       20609 :       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       20609 :     if (fabs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
+     921       20609 :       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       20609 :     initial_z << uav_state.pose.position.z, velocity, acceleration;
+     930             :   }
+     931             : 
+     932             :   //}
+     933             : 
+     934             :   // | ---------------------- set reference --------------------- |
+     935             : 
+     936       41218 :   Eigen::MatrixXd mpc_reference_x = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     937       41218 :   Eigen::MatrixXd mpc_reference_y = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     938       41218 :   Eigen::MatrixXd mpc_reference_z = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     939             : 
+     940             :   // prepare the full reference vector
+     941       20609 :   if (tracker_command.use_full_state_prediction) {
+     942             : 
+     943       11967 :     mpc_reference_x(0, 0) = tracker_command.position.x;
+     944       11967 :     mpc_reference_y(0, 0) = tracker_command.position.y;
+     945       11967 :     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      311142 :     for (int i = 1; i < _horizon_length_; i++) {
+     951      299175 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].x;
+     952      299175 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].y;
+     953      299175 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].z;
+     954             :     }
+     955             : 
+     956      311142 :     for (int i = 1; i < _horizon_length_; i++) {
+     957      299175 :       mpc_reference_x((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].x;
+     958      299175 :       mpc_reference_y((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].y;
+     959      299175 :       mpc_reference_z((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].z;
+     960             :     }
+     961             : 
+     962      311142 :     for (int i = 1; i < _horizon_length_; i++) {
+     963      299175 :       mpc_reference_x((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].x;
+     964      299175 :       mpc_reference_y((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].y;
+     965      299175 :       mpc_reference_z((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].z;
+     966             :     }
+     967             : 
+     968             :   } else {
+     969             : 
+     970      233334 :     for (int i = 0; i < _horizon_length_; i++) {
+     971      224692 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.position.x;
+     972      224692 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.position.y;
+     973      224692 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.position.z;
+     974             :     }
+     975             :   }
+     976             : 
+     977             :   // | ------------------ set the penalizations ----------------- |
+     978             : 
+     979       41218 :   std::vector<double> temp_Q_horizontal = _mat_Q_;
+     980       41218 :   std::vector<double> temp_Q_vertical   = _mat_Q_z_;
+     981             : 
+     982       41218 :   std::vector<double> temp_S_horizontal = _mat_S_;
+     983       41218 :   std::vector<double> temp_S_vertical   = _mat_S_z_;
+     984             : 
+     985       20609 :   if (!tracker_command.use_position_horizontal) {
+     986           0 :     temp_Q_horizontal[0] = 0;
+     987           0 :     temp_S_horizontal[0] = 0;
+     988             :   }
+     989             : 
+     990       20609 :   if (!tracker_command.use_velocity_horizontal) {
+     991           0 :     temp_Q_horizontal[1] = 0;
+     992           0 :     temp_S_horizontal[1] = 0;
+     993             :   }
+     994             : 
+     995       20609 :   if (!tracker_command.use_position_vertical) {
+     996           0 :     temp_Q_vertical[0] = 0;
+     997           0 :     temp_S_vertical[0] = 0;
+     998             :   }
+     999             : 
+    1000       20609 :   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       20609 :   mpc_solver_x_->setQ(temp_Q_horizontal);
+    1008       20609 :   mpc_solver_x_->setS(temp_S_horizontal);
+    1009       20609 :   mpc_solver_x_->setParams();
+    1010       20609 :   mpc_solver_x_->setLastInput(mpc_solver_x_u_);
+    1011       20609 :   mpc_solver_x_->loadReference(mpc_reference_x);
+    1012       20609 :   mpc_solver_x_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1013       20609 :   mpc_solver_x_->setInitialState(initial_x);
+    1014       20609 :   [[maybe_unused]] int iters_x = mpc_solver_x_->solveMPC();
+    1015       20609 :   mpc_solver_x_u_              = mpc_solver_x_->getFirstControlInput();
+    1016             : 
+    1017       20609 :   mpc_solver_y_->setQ(temp_Q_horizontal);
+    1018       20609 :   mpc_solver_y_->setS(temp_S_horizontal);
+    1019       20609 :   mpc_solver_y_->setParams();
+    1020       20609 :   mpc_solver_y_->setLastInput(mpc_solver_y_u_);
+    1021       20609 :   mpc_solver_y_->loadReference(mpc_reference_y);
+    1022       20609 :   mpc_solver_y_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1023       20609 :   mpc_solver_y_->setInitialState(initial_y);
+    1024       20609 :   [[maybe_unused]] int iters_y = mpc_solver_y_->solveMPC();
+    1025       20609 :   mpc_solver_y_u_              = mpc_solver_y_->getFirstControlInput();
+    1026             : 
+    1027       20609 :   mpc_solver_z_->setQ(temp_Q_vertical);
+    1028       20609 :   mpc_solver_z_->setS(temp_S_vertical);
+    1029       20609 :   mpc_solver_z_->setParams();
+    1030       20609 :   mpc_solver_z_->setLastInput(mpc_solver_z_u_);
+    1031       20609 :   mpc_solver_z_->loadReference(mpc_reference_z);
+    1032       20609 :   mpc_solver_z_->setLimits(max_speed_vertical, max_acceleration_vertical, max_u_vertical, 999.0, _dt1_, _dt2_);
+    1033       20609 :   mpc_solver_z_->setInitialState(initial_z);
+    1034       20609 :   [[maybe_unused]] int iters_z = mpc_solver_z_->solveMPC();
+    1035       20609 :   mpc_solver_z_u_              = mpc_solver_z_->getFirstControlInput();
+    1036             : 
+    1037             :   // | ----------- disable lateral feedback if needed ----------- |
+    1038             : 
+    1039       20609 :   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       20609 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+    1047             : 
+    1048       20609 :   Eigen::Array3d Kw(0, 0, 0);
+    1049       20609 :   Eigen::Array3d Kq;
+    1050             : 
+    1051             :   {
+    1052       20609 :     std::scoped_lock lock(mutex_gains_);
+    1053             : 
+    1054       20609 :     Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+    1055             : 
+    1056       20609 :     Kw[0] = gains.kw_rp;
+    1057       20609 :     Kw[1] = gains.kw_rp;
+    1058       20609 :     Kw[2] = gains.kw_y;
+    1059             :   }
+    1060             : 
+    1061             :   // | ---------- desired orientation matrix and force ---------- |
+    1062             : 
+    1063             :   // get body integral in the world frame
+    1064             : 
+    1065       20609 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+    1066             : 
+    1067             :   {
+    1068             : 
+    1069       41218 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+    1070             : 
+    1071       20609 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+    1072       20609 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+    1073       20609 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+    1074       20609 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+    1075       20609 :     Ib_b_stamped.vector.z        = 0;
+    1076             : 
+    1077       41218 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+    1078             : 
+    1079       20609 :     if (res) {
+    1080       20609 :       Ib_w[0] = res.value().vector.x;
+    1081       20609 :       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       20609 :   if (tracker_command.use_acceleration && !tracker_command.use_full_state_prediction) {
+    1090        1958 :     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       18651 :     Ra << mpc_solver_x_u_, mpc_solver_y_u_, mpc_solver_z_u_;
+    1093             :   }
+    1094             : 
+    1095       20609 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+    1096             : 
+    1097       20609 :   Eigen::Vector3d feed_forward = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+    1098             : 
+    1099       20609 :   Eigen::Vector3d integral_feedback;
+    1100             :   {
+    1101       20609 :     std::scoped_lock lock(mutex_integrals_);
+    1102             : 
+    1103       20609 :     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       41218 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+    1118             : 
+    1119       20609 :     Eigen::Vector3d integration_switch(1, 1, 0);
+    1120             : 
+    1121             :     // integrate the world error
+    1122             : 
+    1123             :     // antiwindup
+    1124       20609 :     double temp_gain = gains.kiwxy;
+    1125       20609 :     if (!tracker_command.disable_antiwindups) {
+    1126       18028 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1127        9555 :         temp_gain = 0;
+    1128        9555 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for world integral kicks in", this->name_.c_str());
+    1129             :       }
+    1130             :     }
+    1131             : 
+    1132       20609 :     if (integral_terms_enabled_) {
+    1133       20609 :       if (tracker_command.use_position_horizontal) {
+    1134       20609 :         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       20609 :     bool world_integral_saturated = false;
+    1142       20609 :     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       20609 :     } else if (Iw_w_[0] > gains.kiwxy_lim) {
+    1146           0 :       Iw_w_[0]                 = gains.kiwxy_lim;
+    1147           0 :       world_integral_saturated = true;
+    1148       20609 :     } 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       20609 :     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       20609 :     world_integral_saturated = false;
+    1159       20609 :     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       20609 :     } else if (Iw_w_[1] > gains.kiwxy_lim) {
+    1163           0 :       Iw_w_[1]                 = gains.kiwxy_lim;
+    1164           0 :       world_integral_saturated = true;
+    1165       20609 :     } 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       20609 :     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       41218 :     std::scoped_lock lock(mutex_gains_);
+    1181             : 
+    1182       20609 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+    1183       20609 :     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       41218 :       geometry_msgs::Vector3Stamped Ep_stamped;
+    1189             : 
+    1190       20609 :       Ep_stamped.header.stamp    = ros::Time::now();
+    1191       20609 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+    1192       20609 :       Ep_stamped.vector.x        = Ep(0);
+    1193       20609 :       Ep_stamped.vector.y        = Ep(1);
+    1194       20609 :       Ep_stamped.vector.z        = Ep(2);
+    1195             : 
+    1196       61827 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+    1197             : 
+    1198       20609 :       if (res) {
+    1199       20609 :         Ep_fcu_untilted[0] = res.value().vector.x;
+    1200       20609 :         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       41218 :       geometry_msgs::Vector3Stamped Ev_stamped;
+    1209             : 
+    1210       20609 :       Ev_stamped.header.stamp    = ros::Time::now();
+    1211       20609 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+    1212       20609 :       Ev_stamped.vector.x        = Ev(0);
+    1213       20609 :       Ev_stamped.vector.y        = Ev(1);
+    1214       20609 :       Ev_stamped.vector.z        = Ev(2);
+    1215             : 
+    1216       61827 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+    1217             : 
+    1218       20609 :       if (res) {
+    1219       20609 :         Ev_fcu_untilted[0] = res.value().vector.x;
+    1220       20609 :         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       20609 :     double temp_gain = gains.kibxy;
+    1230       20609 :     if (!tracker_command.disable_antiwindups) {
+    1231       18028 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1232        9555 :         temp_gain = 0;
+    1233        9555 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for body integral kicks in", this->name_.c_str());
+    1234             :       }
+    1235             :     }
+    1236             : 
+    1237       20609 :     if (integral_terms_enabled_) {
+    1238       20609 :       if (tracker_command.use_position_horizontal) {
+    1239       20609 :         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       20609 :     bool body_integral_saturated = false;
+    1247       20609 :     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       20609 :     } else if (Ib_b_[0] > gains.kibxy_lim) {
+    1251           0 :       Ib_b_[0]                = gains.kibxy_lim;
+    1252           0 :       body_integral_saturated = true;
+    1253       20609 :     } 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       20609 :     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       20609 :     body_integral_saturated = false;
+    1264       20609 :     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       20609 :     } else if (Ib_b_[1] > gains.kibxy_lim) {
+    1268           0 :       Ib_b_[1]                = gains.kibxy_lim;
+    1269           0 :       body_integral_saturated = true;
+    1270       20609 :     } 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       20609 :     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       20609 :   Eigen::Vector3d des_acc = integral_feedback / total_mass + Ra;
+    1283             : 
+    1284       20609 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1285             : 
+    1286         695 :     if (output_modality == common::ACCELERATION_HDG) {
+    1287             : 
+    1288         738 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1289             : 
+    1290         369 :       cmd.acceleration.x = des_acc[0];
+    1291         369 :       cmd.acceleration.y = des_acc[1];
+    1292         369 :       cmd.acceleration.z = des_acc[2];
+    1293             : 
+    1294         369 :       cmd.heading = tracker_command.heading;
+    1295             : 
+    1296         369 :       last_control_output_.control_output = cmd;
+    1297             : 
+    1298             :     } else {
+    1299             : 
+    1300         326 :       double des_hdg_ff = 0;
+    1301             : 
+    1302         326 :       if (tracker_command.use_heading_rate) {
+    1303         326 :         des_hdg_ff = tracker_command.heading_rate;
+    1304             :       }
+    1305             : 
+    1306         652 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1307             : 
+    1308         326 :       cmd.acceleration.x = des_acc[0];
+    1309         326 :       cmd.acceleration.y = des_acc[1];
+    1310         326 :       cmd.acceleration.z = des_acc[2];
+    1311             : 
+    1312         326 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1313             : 
+    1314         326 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1315             : 
+    1316         326 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1317             : 
+    1318         326 :       cmd.heading_rate = des_hdg_rate;
+    1319             : 
+    1320         326 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1321             : 
+    1322         326 :       last_control_output_.control_output = cmd;
+    1323             :     }
+    1324             : 
+    1325             :     // | -------------- unbiased desired acceleration ------------- |
+    1326             : 
+    1327         695 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1328             : 
+    1329             :     {
+    1330             : 
+    1331        1390 :       geometry_msgs::Vector3Stamped world_accel;
+    1332             : 
+    1333         695 :       world_accel.header.stamp    = ros::Time::now();
+    1334         695 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1335         695 :       world_accel.vector.x        = Ra[0];
+    1336         695 :       world_accel.vector.y        = Ra[1];
+    1337         695 :       world_accel.vector.z        = Ra[2];
+    1338             : 
+    1339        2085 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1340             : 
+    1341         695 :       if (res) {
+    1342         695 :         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         695 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1348             : 
+    1349             :     // | ----------------- fill in the diagnostics ---------------- |
+    1350             : 
+    1351         695 :     last_control_output_.diagnostics.ramping_up = false;
+    1352             : 
+    1353         695 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1354         695 :     last_control_output_.diagnostics.mass_difference = 0;
+    1355         695 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1356             : 
+    1357         695 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1358             : 
+    1359         695 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1360         695 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1361             : 
+    1362         695 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1363         695 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1364             : 
+    1365         695 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1366         695 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1367             : 
+    1368         695 :     last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1369             : 
+    1370         695 :     last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * max_speed_horizontal;
+    1371         695 :     last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * max_acceleration_horizontal;
+    1372             : 
+    1373         695 :     last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * max_speed_vertical;
+    1374         695 :     last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1375             : 
+    1376         695 :     last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * max_speed_vertical;
+    1377         695 :     last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1378             : 
+    1379         695 :     last_control_output_.diagnostics.controller = name_;
+    1380             : 
+    1381         695 :     return;
+    1382             :   }
+    1383             : 
+    1384             :   /* mass estimatior //{ */
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                integrate the mass difference               |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390             :   {
+    1391       39828 :     std::scoped_lock lock(mutex_gains_);
+    1392             : 
+    1393             :     // antiwindup
+    1394       19914 :     double temp_gain = gains.km;
+    1395       39539 :     if (rampup_active_ ||
+    1396       19625 :         (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        6936 :       temp_gain = 0;
+    1398        6936 :       ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for the mass kicks in", this->name_.c_str());
+    1399             :     }
+    1400             : 
+    1401       19914 :     if (tracker_command.use_position_vertical) {
+    1402       19914 :       uav_mass_difference_ += temp_gain * Ep[2] * dt;
+    1403             :     }
+    1404             : 
+    1405             :     // saturate the mass estimator
+    1406       19914 :     bool uav_mass_saturated = false;
+    1407       19914 :     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       19914 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1411           0 :       uav_mass_difference_ = gains.km_lim;
+    1412           0 :       uav_mass_saturated   = true;
+    1413       19914 :     } 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       19914 :     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       19914 :   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       19914 :   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       19914 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1441             : 
+    1442       19914 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "MpcController");
+    1443             : 
+    1444       19914 :   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       19914 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1458             : 
+    1459             :   // --------------------------------------------------------------
+    1460             :   // |               desired orientation + throttle               |
+    1461             :   // --------------------------------------------------------------
+    1462             : 
+    1463             :   // | ------------------- desired orientation ------------------ |
+    1464             : 
+    1465       19914 :   Eigen::Matrix3d Rd;
+    1466             : 
+    1467       19914 :   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       19914 :     Eigen::Vector3d bxd;  // desired heading vector
+    1484             : 
+    1485       19914 :     if (tracker_command.use_heading) {
+    1486       19914 :       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       19914 :     Rd = common::so3transform(f_normed, bxd, false);
+    1493             :   }
+    1494             : 
+    1495             :   // | -------------------- desired throttle -------------------- |
+    1496             : 
+    1497       19914 :   double desired_thrust_force = f.dot(R.col(2));
+    1498       19914 :   double throttle             = 0;
+    1499             : 
+    1500       19914 :   if (rampup_active_) {
+    1501             : 
+    1502             :     // deactivate the rampup when the times up
+    1503         289 :     if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1504             : 
+    1505          24 :       rampup_active_ = false;
+    1506             : 
+    1507          24 :       ROS_INFO("[%s]: rampup finished", name_.c_str());
+    1508             : 
+    1509             :     } else {
+    1510             : 
+    1511         265 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1512             : 
+    1513         265 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1514             : 
+    1515         265 :       rampup_last_time_ = ros::Time::now();
+    1516             : 
+    1517         265 :       throttle = rampup_throttle_;
+    1518             : 
+    1519         265 :       ROS_INFO_THROTTLE(0.1, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+    1520             :     }
+    1521             : 
+    1522             :   } else {
+    1523             : 
+    1524       19625 :     if (desired_thrust_force >= 0) {
+    1525       19625 :       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       19914 :   bool throttle_saturated = false;
+    1534             : 
+    1535       19914 :   if (!std::isfinite(throttle)) {
+    1536             : 
+    1537           0 :     ROS_ERROR("[%s]: NaN detected in variable 'throttle'!!!", name_.c_str());
+    1538           0 :     return;
+    1539             : 
+    1540       19914 :   } 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       19914 :   } 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       19914 :   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       19914 :   double desired_x_accel = 0;
+    1569       19914 :   double desired_y_accel = 0;
+    1570       19914 :   double desired_z_accel = 0;
+    1571             : 
+    1572             :   {
+    1573       19914 :     Eigen::Vector3d thrust_vector = desired_thrust_force * Rd.col(2);
+    1574             : 
+    1575       19914 :     double world_accel_x = (thrust_vector[0] / total_mass) - (Iw_w_[0] / total_mass) - (Ib_w[0] / total_mass);
+    1576       19914 :     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       19914 :     double world_accel_z = tracker_command.acceleration.z;
+    1580             : 
+    1581       39828 :     geometry_msgs::Vector3Stamped world_accel;
+    1582             : 
+    1583       19914 :     world_accel.header.stamp    = ros::Time::now();
+    1584       19914 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1585       19914 :     world_accel.vector.x        = world_accel_x;
+    1586       19914 :     world_accel.vector.y        = world_accel_y;
+    1587       19914 :     world_accel.vector.z        = world_accel_z;
+    1588             : 
+    1589       59742 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1590             : 
+    1591       19914 :     if (res) {
+    1592             : 
+    1593       19914 :       desired_x_accel = res.value().vector.x;
+    1594       19914 :       desired_y_accel = res.value().vector.y;
+    1595       19914 :       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       19914 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1603             : 
+    1604             :   // fill the unbiased desired accelerations
+    1605       19914 :   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       19914 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1610             : 
+    1611       19914 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1612       19914 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1613       19914 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1614             : 
+    1615       19914 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1616             : 
+    1617       19914 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1618       19914 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1619             : 
+    1620       19914 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1621       19914 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1622             : 
+    1623       19914 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1624       19914 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1625             : 
+    1626       19914 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1627             : 
+    1628       19914 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1629       19914 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1630             : 
+    1631       19914 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1632       19914 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1633             : 
+    1634       19914 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1635       19914 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1636             : 
+    1637       19914 :   last_control_output_.diagnostics.controller = name_;
+    1638             : 
+    1639             :   // | ------------ construct the attitude reference ------------ |
+    1640             : 
+    1641       19914 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1642             : 
+    1643       19914 :   attitude_cmd.stamp       = ros::Time::now();
+    1644       19914 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1645       19914 :   attitude_cmd.throttle    = throttle;
+    1646             : 
+    1647       19914 :   if (output_modality == common::ATTITUDE) {
+    1648             : 
+    1649         833 :     last_control_output_.control_output = attitude_cmd;
+    1650             : 
+    1651         833 :     return;
+    1652             :   }
+    1653             : 
+    1654             :   // --------------------------------------------------------------
+    1655             :   // |                      attitude control                      |
+    1656             :   // --------------------------------------------------------------
+    1657             : 
+    1658       19081 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1659             : 
+    1660       19081 :   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       19081 :   } else if (tracker_command.use_heading_rate) {
+    1665             : 
+    1666             :     // to fill in the feed forward yaw rate
+    1667       19079 :     double desired_yaw_rate = 0;
+    1668             : 
+    1669             :     try {
+    1670       19079 :       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       19079 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1677             :   }
+    1678             : 
+    1679             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1680             : 
+    1681       19081 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1682             : 
+    1683       19081 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1684             : 
+    1685       10440 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: using jerk feedforward", name_.c_str());
+    1686             : 
+    1687       10440 :     Eigen::Matrix3d I;
+    1688       10440 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1689       10440 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1690       10440 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1691             :   }
+    1692             : 
+    1693             :   // | --------------- run the attitude controller -------------- |
+    1694             : 
+    1695       19081 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1696             : 
+    1697       19081 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq, false);
+    1698             : 
+    1699       19081 :   if (!attitude_rate_command) {
+    1700           0 :     return;
+    1701             :   }
+    1702             : 
+    1703             :   // | --------- fill in the already known attitude rate -------- |
+    1704             : 
+    1705             :   {
+    1706             :     try {
+    1707       19081 :       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       19081 :   if (output_modality == common::ATTITUDE_RATE) {
+    1716             : 
+    1717       16809 :     last_control_output_.control_output = attitude_rate_command;
+    1718             : 
+    1719       16809 :     return;
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                    Attitude rate control                   |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726        2272 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1727             : 
+    1728        2272 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1729             : 
+    1730        2272 :   if (!control_group_command) {
+    1731           0 :     return;
+    1732             :   }
+    1733             : 
+    1734        2272 :   if (output_modality == common::CONTROL_GROUP) {
+    1735             : 
+    1736        1208 :     last_control_output_.control_output = control_group_command;
+    1737             : 
+    1738        1208 :     return;
+    1739             :   }
+    1740             : 
+    1741             :   // --------------------------------------------------------------
+    1742             :   // |                        output mixer                        |
+    1743             :   // --------------------------------------------------------------
+    1744             : 
+    1745        2128 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1746             : 
+    1747        1064 :   last_control_output_.control_output = actuator_cmd;
+    1748             : 
+    1749        1064 :   return;
+    1750             : }
+    1751             : 
+    1752             : //}
+    1753             : 
+    1754             : /* positionPassthrough() //{ */
+    1755             : 
+    1756         181 : void MpcController::positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    1757             : 
+    1758         181 :   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         362 :   mrs_msgs::HwApiPositionCmd cmd;
+    1764             : 
+    1765         181 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1766         181 :   cmd.header.stamp    = ros::Time::now();
+    1767             : 
+    1768         181 :   cmd.position = tracker_command.position;
+    1769         181 :   cmd.heading  = tracker_command.heading;
+    1770             : 
+    1771         181 :   last_control_output_.control_output = cmd;
+    1772             : 
+    1773             :   // fill the unbiased desired accelerations
+    1774         181 :   last_control_output_.desired_unbiased_acceleration = {};
+    1775         181 :   last_control_output_.desired_orientation           = {};
+    1776         181 :   last_control_output_.desired_heading_rate          = {};
+    1777             : 
+    1778             :   // | ----------------- fill in the diagnostics ---------------- |
+    1779             : 
+    1780         181 :   last_control_output_.diagnostics.ramping_up = false;
+    1781             : 
+    1782         181 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1783         181 :   last_control_output_.diagnostics.mass_difference = 0;
+    1784             : 
+    1785         181 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1786             : 
+    1787         181 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1788         181 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1789             : 
+    1790         181 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1791         181 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1792             : 
+    1793         181 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1794         181 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1795             : 
+    1796         181 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1797             : 
+    1798         181 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1799         181 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1800             : 
+    1801         181 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1802         181 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1803             : 
+    1804         181 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1805         181 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1806             : 
+    1807         181 :   last_control_output_.diagnostics.controller = name_;
+    1808             : }
+    1809             : 
+    1810             : //}
+    1811             : 
+    1812             : /* PIDVelocityOutput() //{ */
+    1813             : 
+    1814         330 : 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         330 :   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         330 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1823             : 
+    1824         330 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1825         330 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1826             : 
+    1827         330 :   double hdg_ref = tracker_command.heading;
+    1828         330 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1829             : 
+    1830             :   // | ------------------ velocity feedforward ------------------ |
+    1831             : 
+    1832         330 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1833             : 
+    1834         330 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1835         330 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1836             :   }
+    1837             : 
+    1838             :   // | --------------------- control errors --------------------- |
+    1839             : 
+    1840         330 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1841             : 
+    1842             :   // | --------------------------- pid -------------------------- |
+    1843             : 
+    1844         330 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1845         330 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1846         330 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1847             : 
+    1848         330 :   double des_vel_x = position_pid_x_.update(Ep[0], dt);
+    1849         330 :   double des_vel_y = position_pid_y_.update(Ep[1], dt);
+    1850         330 :   double des_vel_z = position_pid_z_.update(Ep[2], dt);
+    1851             : 
+    1852             :   // | -------------------- position feedback ------------------- |
+    1853             : 
+    1854         330 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1855             : 
+    1856         330 :   if (control_output == common::VELOCITY_HDG) {
+    1857             : 
+    1858             :     // | --------------------- fill the output -------------------- |
+    1859             : 
+    1860         338 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1861             : 
+    1862         169 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1863         169 :     cmd.header.stamp    = ros::Time::now();
+    1864             : 
+    1865         169 :     cmd.velocity.x = des_vel[0];
+    1866         169 :     cmd.velocity.y = des_vel[1];
+    1867         169 :     cmd.velocity.z = des_vel[2];
+    1868             : 
+    1869         169 :     cmd.heading = tracker_command.heading;
+    1870             : 
+    1871         169 :     last_control_output_.control_output = cmd;
+    1872             : 
+    1873         161 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1874             : 
+    1875         161 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1876             : 
+    1877         161 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1878             : 
+    1879         161 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1880             : 
+    1881             :     // | --------------------------- ff --------------------------- |
+    1882             : 
+    1883         161 :     double des_hdg_ff = 0;
+    1884             : 
+    1885         161 :     if (tracker_command.use_heading_rate) {
+    1886         161 :       des_hdg_ff = tracker_command.heading_rate;
+    1887             :     }
+    1888             : 
+    1889             :     // | --------------------- fill the output -------------------- |
+    1890             : 
+    1891         322 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1892             : 
+    1893         161 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1894         161 :     cmd.header.stamp    = ros::Time::now();
+    1895             : 
+    1896         161 :     cmd.velocity.x = des_vel[0];
+    1897         161 :     cmd.velocity.y = des_vel[1];
+    1898         161 :     cmd.velocity.z = des_vel[2];
+    1899             : 
+    1900         161 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1901             : 
+    1902         161 :     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         330 :   last_control_output_.desired_unbiased_acceleration = {};
+    1911         330 :   last_control_output_.desired_orientation           = {};
+    1912         330 :   last_control_output_.desired_heading_rate          = {};
+    1913             : 
+    1914             :   // | ----------------- fill in the diagnostics ---------------- |
+    1915             : 
+    1916         330 :   last_control_output_.diagnostics.ramping_up = false;
+    1917             : 
+    1918         330 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1919         330 :   last_control_output_.diagnostics.mass_difference = 0;
+    1920             : 
+    1921         330 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1922             : 
+    1923         330 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1924         330 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1925             : 
+    1926         330 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1927         330 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1928             : 
+    1929         330 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1930         330 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1931             : 
+    1932         330 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1933             : 
+    1934         330 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1935         330 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1936             : 
+    1937         330 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1938         330 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1939             : 
+    1940         330 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1941         330 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1942             : 
+    1943         330 :   last_control_output_.diagnostics.controller = name_;
+    1944             : }
+    1945             : 
+    1946             : //}
+    1947             : 
+    1948             : // --------------------------------------------------------------
+    1949             : // |                          callbacks                         |
+    1950             : // --------------------------------------------------------------
+    1951             : 
+    1952             : /* //{ callbackDrs() */
+    1953             : 
+    1954          84 : void MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, [[maybe_unused]] uint32_t level) {
+    1955             : 
+    1956          84 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1957             : 
+    1958          84 :   ROS_INFO("[%s]: DRS updated gains", this->name_.c_str());
+    1959          84 : }
+    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        4828 : void MpcController::timerGains(const ros::TimerEvent &event) {
+    1993             : 
+    1994       14484 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1995             :   mrs_lib::ScopeTimer timer =
+    1996       14484 :       mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1997             : 
+    1998        9656 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1999        4828 :   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        4828 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    2004        4828 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    2005             : 
+    2006        4828 :   mute_gains_ = false;
+    2007             : 
+    2008        4828 :   const double dt = (event.current_real - event.last_real).toSec();
+    2009             : 
+    2010        4828 :   bool updated = false;
+    2011             : 
+    2012        4828 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    2013        4828 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    2014        4828 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    2015        4828 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    2016        4828 :   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        4828 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    2020        4828 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    2021        4828 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    2022             : 
+    2023        4828 :   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        4828 :   if (updated) {
+    2028             : 
+    2029         639 :     drs_params.kiwxy         = gains.kiwxy;
+    2030         639 :     drs_params.kibxy         = gains.kibxy;
+    2031         639 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    2032         639 :     drs_params.kq_yaw        = gains.kq_yaw;
+    2033         639 :     drs_params.km            = gains.km;
+    2034         639 :     drs_params.km_lim        = gains.km_lim;
+    2035         639 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    2036         639 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    2037             : 
+    2038         639 :     drs_->updateConfig(drs_params);
+    2039             : 
+    2040         639 :     ROS_INFO_THROTTLE(10.0, "[%s]: gains have been updated", name_.c_str());
+    2041             :   }
+    2042        4828 : }
+    2043             : 
+    2044             : //}
+    2045             : 
+    2046             : // --------------------------------------------------------------
+    2047             : // |                       other routines                       |
+    2048             : // --------------------------------------------------------------
+    2049             : 
+    2050             : /* calculateGainChange() //{ */
+    2051             : 
+    2052       38624 : 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       38624 :   double change = desired_value - current_value;
+    2056             : 
+    2057       38624 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    2058       38624 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    2059             : 
+    2060       38624 :   if (!bypass_rate) {
+    2061             : 
+    2062             :     // if current value is near 0...
+    2063             :     double change_in_perc;
+    2064             :     double saturated_change;
+    2065             : 
+    2066       38269 :     if (fabs(current_value) < 1e-6) {
+    2067           0 :       change *= gains_filter_max_change;
+    2068             :     } else {
+    2069             : 
+    2070       38269 :       saturated_change = change;
+    2071             : 
+    2072       38269 :       change_in_perc = (current_value + saturated_change) / current_value - 1.0;
+    2073             : 
+    2074       38269 :       if (change_in_perc > gains_filter_max_change) {
+    2075        2485 :         saturated_change = current_value * gains_filter_max_change;
+    2076       35784 :       } else if (change_in_perc < -gains_filter_max_change) {
+    2077           0 :         saturated_change = current_value * -gains_filter_max_change;
+    2078             :       }
+    2079             : 
+    2080       38269 :       if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
+    2081           0 :         change *= gains_filter_min_change;
+    2082             :       } else {
+    2083       38269 :         change = saturated_change;
+    2084             :       }
+    2085             :     }
+    2086             :   }
+    2087             : 
+    2088       38624 :   if (fabs(change) > 1e-3) {
+    2089        3195 :     ROS_DEBUG("[%s]: changing gain '%s' from %.2f to %.2f", name_.c_str(), name.c_str(), current_value, desired_value);
+    2090        3195 :     updated = true;
+    2091             :   }
+    2092             : 
+    2093       38624 :   return current_value + change;
+    2094             : }
+    2095             : 
+    2096             : //}
+    2097             : 
+    2098             : /* getHeadingSafely() //{ */
+    2099             : 
+    2100       20939 : double MpcController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    2101             : 
+    2102             :   try {
+    2103       20939 :     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          42 : 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..86eb361dfb14f3b34fba5b6ed313bb608d9474a9 GIT binary patch literal 6650 zcmVe90{{R30vG`D0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp!q=neQq=lq|T>q`SoACz-3XWc~`{H}In`8+JGKhe9xh~i1b^ZI_ zWmNl%??8HOq0E3yP;1w#7H~|z7fj4`2vG=Qi=f^y@_0F7&W!_O96^m5BLY-o)OT&- zqk^z%<7Va@H^+z;!niAx&=xIV6|@I%9Yfa@^o%bIQCJrA?G;N&hcYEH)d1XI3{ydw znB@v|&IvOWPC@yH6}@Mlu9bdS0=&>J&>!(1R`R|*_-m3L!7AzFK9-&Z{Oo5qQhS1~ z?^-mg2rD2DWH{NPx`u`g!SziH3hrQ%f2~A2BMw z8Q22!&qaZ8J;jX@Bbe)@6tUj9UJ}iIgBE@qtX~2JSguw8=r=4&+LCp4T?4%2rDKJ^ z&E)2;;Um(E24aLy;S!`3ba6s(^L(d8J8upwc&~!qAtw8YF^b5jWEcs4uwsa}G=vl%*rN@qy( zDp7&ZGjv_RYTgE12iVNPT^J;*iVMiiQgthi2UIukc#J@I3Go7rJd?+Y>hnA({Ktg> zvkrfMW|pX9<6vGm%W!X+rOm-kj-TH{$_+ z+Zf?&zY$P`9UJ66FwO{cSKFEa(|5qmJ&$KKW3-GhcG*oQZuh&pCvxULA1OENkQT`R69aPH%8!<*@A3X$1 z@)`$A+bp?YjFRLyPRz7aQ#_fc7L5GR90hzl{)G12|9}2@{r&G~;n$znV{abt8Q;q7 z@@PRsKz)Pj^%|${Ok6=^IB9PMG)Rws^5gRlAIPi}3>dwUp0=a%jPw#sCCii2A0dasIN=#hbK6#7_kQjnzm1$8RDn>h zll?q-z4kg4CB=NidOe)#pD{+!19pcl1BJ7|`wKqASr9|_Cv&ek_GC4lUmXmW0rP9B z><{I>^Yaydghk*(l)ZBm&t`A}9u0c}`vJ9rOA~>X(+O39>J7S!xuzUreq28rqq0GU zFG+Wwe7Hh8L}LT5N!NBj%A`#M1Q;2ga{}~lHvy~%qaT&*w>JPPTf&&0K*z@m%5|oN zapyKGle>xg_dh~RZH!uCN*_QL%Mmt(^o(tv;B+bCE|B4S>mB2Wh&X4ZxWK#c#_b~Y zj{rH??kOCQ-*b#nmaa79?lCUW?m0h&8yOXSdhNPrC%mQsY#3cFP#6Ifu~`qOZnKW3 z4-ANx5pjop9WU(+2VW)uN(gAd=rBG3P>)f$pMmDOtW6I^8mWY+*)b--t4Blma1*M`Y-~)ERdTp0(HC< zxgG!z<_uLaZkU74ZTwyVVU!<TKeq7raC5>|iV{@me7Bh9%6LYVJ66L@X#xF6TYw;th5+@%L{CryFh(F_dru@6oT5GB+=rDp$273zVs}PhX5vKE?K5It z*S1o+os{1f!snPg)DWWr-L1=DAGj*G#G5fLWEhNp`b383@$X1(NdND_c%_8@#kePI zDFEj^GYibG0jZijQ2^qdx=Xf(TK5`jvpbH|ph=OhAQN6*>E^#)zTl_d;10V6akYXMNz^$c$z4X5dd$9USeGoAf!7Zr3bRfXbAV!!Q1u=Om^%2DA6aYs|0r-AmDke>HW;P7? zCFV;EXx;*Nwiout0jaP69!gtbLJ*nQ6f%2$R9s=$%_+q)3f}-R?oQjGf_)ZaC2yxM z5onCVS3iYQE3k|S1n?)r@`qrDq-&Q4{N}6NYsIaV38O#Jzzp>T_AGjgVK*Ec81eim zsPSl?xd%I}Qe)crm=Fm&QwoSTb6^CWU3;#N1r(PEqfl@Xf!fm~xKGVeXc;4W{PH}; zXVZsd0aSLq-^EnDgU#GHKgBa0Uh+EQX`8}n3~!&6QUhvzDM~m4G1E(e4Oo+e1?dlc zG&{6eT_$W8v62<0N)Py>=6YZXf}HdSty7ddCw^tKl~z-R3#iYYwrn8%o0IXbeAAf{ zQ)cZ0Kr2QvM(NC1>x>2hOrsM3En}<%nUX;LxGezJxFU>~ScegQGPelhb?B9!Bv~n| z02hC9htsgGz^*}k*C~sH4OF`Hn~CAMoknjM6xwKt-=^5%sSiv>^2FhB#<*W+%C zaeE&R+Zo*QeD(xQm|xeoI{;ftfT%9Edu=o&6U2E7ip2b*RW z-F?sGH9{Eq2$uhjX|i>VV>o5EyX9PkW_5lu!wI{2KAvZIX7{tf@E35|O=0*Qr-LT_ zf>=5sD}I)*>8s5|z^^v@YO}94`)V^q-(PJecm3670=rWct*We)$ib^+FEx^PbndrXT5loJMMb0lOd0M^Fnd?8G{Nn-$uBq;)ZE`{|w zGbP54m>J;`>OR0vn3=*G@grsiRs>4_zy-=P?H{MgtWLvjB$9I#Opc!Hx6-)kSe zLoo}f?>5%pu3J(Ho*4;jz^#~{1V)12nd?dm*%0aK7$Zqn7P7+&K)|CK#+*Fg=-LP< z7y8jt&F7C}=33)i?I=lbkyNhw>HxF`#9%Zfos8XI`znI(EVgkelN3+((I_aF8r9)g z$~OpTDQ20zSYU#HQ;J#o^FBjk8>>2bdoVXQ#o#*t(23HCXCS}xZtUGl}*K0aKD1vEw~dj^NhsZ##$xg8VD!WdXEGGBf3NEig(>C7jVf|sJ++oPrtGUB)*YZ6T05E#F8nuuI z0vf1p`NNFvFr7aPo7a+)!V0MJ&jgEyX6Algkf=c-NABAIISWPmf4vmU_{BirjYSCmgyHX*| z?tP(n>yN(cx%WnKPi%*_kE3?MNhL@+r7<3Y*$9j-fq9KJBxAj0EHDmd%AKAeC|faQ{S( zh))1CP~A-Xv=k1w^|zqPvaC66R6oAM_+PFHAI#ZJy6<%Qlea&E;W$(0W-p5V5eu$}a*v*Z*rfx;HYle1x z*Hhixa+snZCeF+&%t;QvKGRh%m9tSTJl5%=%y1ykbWP@K|Hi<(DlL4(_X4c&ii6X)Kv9=L z*Y!Mw3TK+pyoS2I;)LiBiq|XU_CO{7%K1I{lPohKvIdeT5MYdpa%c(07^ZuIc-=Ds z?hRM@MDMaZLW}^{6AgvM7zXj`l*glMg_KXc-f?3Dw@)ZK40do48`OyZ@<63!WjeCW-3MGn{v}{C0xXWYpQtK#%rW=e2LjI2H!%BIn9)(A1l)mW`svbT= ztJytGYkUUX@Cw@O8P(NWbGc>|icG4)%VzjT1imZ0UtN`sJ=a#ilDC`RbDbz|5dJ-* zNgOg$W7b}n>+ph1>?F&c5E=Hrls(fbutc3eYtNm&CX*>^5QzzZfhN&s(3O|csAQwX zn4B))UN`nMJCSf&Jw5%gCl)6k$FmW0gAaB{mxv$qVVs)E;UqJ=m+Ct`>xR&GVFY_k z9?*nQIw8@IF;W<^zXD_Pyt^6Bs~HOt+|X>2;xG9qwBF^z8vvmkPTtZy-<>j8ZgNlC zxEilwzl|&E=N>;rnO+NiKMZ>=WxH_6vvp1q=HGFF;wlGFy)g8ZCz?gvzxA}0cSyb`eCt{>HD;R^OVqA72w%t$i4;fCqD+Abr zw~G82d15B|9|z^7A@f=)c;^-ActB z3yj6B-8OE{YRoaU6di(aY}O9*+}a%HnuJQR+BfaLBc^#Blu#**aVtCvrhpia6J#n@ z0qL1`X^b?Xa=;j4?*lR7j6ny~)Jl%EavFOl`Yj*pl+>QbIQ9iuWL-p+@R#hq3Q?WE5;3n4FHja@w&&i!_ z)??&P7x^*wV|-usY_)OKY<;@xf=z{aq4VMd6qcYKhBG;coN3(U1S~3ZKsP{-Z?8T~ z+rBVmait4IIflcXcqNC{)U{4w-OBTYY}0Q6lp%JMQAyH;a_-q5TehNF!eL~{Nn6~x zao05^Xsqzv`3rT?Z53|7<4T_jmjTrnnXK=0RTS7UbYN_kH`>B1$KP#?^7DYtruc70 zKoVEH&yCu{GH_`dH$Zwwi=X$UHS%0^W{hHK4-ELmEW0=}DE%rYp7!`D^xv>6do;y# z?x!gGw?|2_VV{$_&)gw~QkKfgLG=~hGkX4ZFN-LD9N~<2hTX6mJs+CMxhQAQFojd^gD;xUw>kF|Gg(5A~VkCAZ3hV+>-#xk<`Af1)od-xFNtHT|;~) zORJ3uh}Fj!*`op6I6wn2|1QMV$#7-Ol4^*p*Xt@yALZ{+M; z*PjqM`(c*UpkgGfB~=SFKynMse~I$&lPZ3fQgITIgKE;p)s zQv#}~rVmX#4B1UKlddcfqo!K96GuV2QD^reza7)zWI)>(TUEi&yJyc}gozhq)j-!0 zo*5$H0QX z?MaI7tM*!e)-k4Qw>FmW8EQ{=zStm-<$2dsiDdsIrb@luoc-|i&Zo&gQ{K&QF}6>r-fp>Cx7dSV_I9n!=Dt@ ziSF+u*Zw4MzQvN6>-s)g(rZ@%c(z zeIf3LA*Y-CtYb_gjaYZnI=p$=sjhLb0IW}4?^nXrcI=UW@gb}!Y3zUyErY;f`690>dB1d0@Yl9Tz^&+V3a%M z&aT(#qb>flY$}w|-y2;u`L^kWLV47FPIzBQb#zKs+siV>H&U&<(B@H6EevJK7|V-F zSd2fy;XD3{oCRLqH$tZnj6M4j(yq|XtPQlOOoDRz59S4z9NylA9RL6T07*qoM6N<$ Ef`yThhX4Qo literal 0 HcmV?d00001 diff --git a/mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.html b/mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.html new file mode 100644 index 0000000000..79ccdbf207 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.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:60977478.7 %
Date:2023-12-06 07:59:45Functions: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()6
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)13
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)84
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)162
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)165
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&)358
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()739
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)1140
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&)8733
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9091
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9256
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)15960
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)31457
+
+
+ + + +
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..0fff711cc1 --- /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:60977478.7 %
Date:2023-12-06 07:59:45Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()6
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>)42
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)1140
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)84
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9256
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&)8733
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)162
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)31457
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9091
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&)358
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)15960
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)165
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&)13
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()739
+
+
+ + + +
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..3c771904c3 --- /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:60977478.7 %
Date:2023-12-06 07:59:45Functions: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          42 : 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          42 :   nh_ = nh;
+     217             : 
+     218          42 :   common_handlers_  = common_handlers;
+     219          42 :   private_handlers_ = private_handlers;
+     220             : 
+     221          42 :   _uav_mass_ = common_handlers->getMass();
+     222             : 
+     223          42 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // | ---------- loading params using the parent's nh ---------- |
+     226             : 
+     227          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     228             : 
+     229          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     230             : 
+     231          42 :   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          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
+     239          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
+     240             : 
+     241          84 :   const std::string yaml_namespace = "mrs_uav_controllers/se3_controller/";
+     242             : 
+     243             :   // lateral gains
+     244          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kp", gains_.kpxy);
+     245          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kv", gains_.kvxy);
+     246          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/ka", gains_.kaxy);
+     247             : 
+     248          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw", gains_.kiwxy);
+     249          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib", gains_.kibxy);
+     250             : 
+     251             :   // | ------------------------- rampup ------------------------- |
+     252             : 
+     253          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/enabled", _rampup_enabled_);
+     254          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/speed", _rampup_speed_);
+     255             : 
+     256             :   // height gains
+     257          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kp", gains_.kpz);
+     258          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kv", gains_.kvz);
+     259          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/ka", gains_.kaz);
+     260             : 
+     261             :   // attitude gains
+     262          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_roll_pitch", gains_.kq_roll_pitch);
+     263          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_yaw", gains_.kq_yaw);
+     264             : 
+     265             :   // attitude rate gains
+     266          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_roll_pitch", gains_.kw_roll_pitch);
+     267          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_yaw", gains_.kw_yaw);
+     268             : 
+     269             :   // mass estimator
+     270          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km", gains_.km);
+     271          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
+     272             : 
+     273             :   // integrator limits
+     274          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
+     275          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib_lim", gains_.kibxy_lim);
+     276             : 
+     277             :   // constraints
+     278          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     279          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     280             : 
+     281          42 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     282             : 
+     283          42 :   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          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/throttle_saturation", _throttle_saturation_);
+     289             : 
+     290             :   // gain filtering
+     291          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     292          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     293          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/rate", _gain_filtering_rate_);
+     294          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     295             : 
+     296             :   // output mode
+     297          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/preferred_output", drs_params_.preferred_output_mode);
+     298             : 
+     299          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rotation_matrix", drs_params_.rotation_type);
+     300             : 
+     301             :   // angular rate feed forward
+     302          84 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/parasitic_pitch_roll",
+     303          42 :                                             drs_params_.pitch_roll_heading_rate_compensation);
+     304          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     305             : 
+     306             :   // | ------------------- position pid params ------------------ |
+     307             : 
+     308          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     309          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     310          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     311             : 
+     312          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     313          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     314          42 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     315             : 
+     316             :   // | ------------------ finish loading params ----------------- |
+     317             : 
+     318          42 :   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          42 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     326          42 :         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          42 :   uav_mass_difference_ = 0;
+     333          42 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     334          42 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     335             : 
+     336             :   // | --------------- dynamic reconfigure server --------------- |
+     337             : 
+     338          42 :   drs_params_.kpxy             = gains_.kpxy;
+     339          42 :   drs_params_.kvxy             = gains_.kvxy;
+     340          42 :   drs_params_.kaxy             = gains_.kaxy;
+     341          42 :   drs_params_.kiwxy            = gains_.kiwxy;
+     342          42 :   drs_params_.kibxy            = gains_.kibxy;
+     343          42 :   drs_params_.kpz              = gains_.kpz;
+     344          42 :   drs_params_.kvz              = gains_.kvz;
+     345          42 :   drs_params_.kaz              = gains_.kaz;
+     346          42 :   drs_params_.kq_roll_pitch    = gains_.kq_roll_pitch;
+     347          42 :   drs_params_.kq_yaw           = gains_.kq_yaw;
+     348          42 :   drs_params_.kiwxy_lim        = gains_.kiwxy_lim;
+     349          42 :   drs_params_.kibxy_lim        = gains_.kibxy_lim;
+     350          42 :   drs_params_.km               = gains_.km;
+     351          42 :   drs_params_.km_lim           = gains_.km_lim;
+     352          42 :   drs_params_.jerk_feedforward = true;
+     353             : 
+     354          42 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     355          42 :   drs_->updateConfig(drs_params_);
+     356          42 :   Drs_t::CallbackType f = boost::bind(&Se3Controller::callbackDrs, this, _1, _2);
+     357          42 :   drs_->setCallback(f);
+     358             : 
+     359             :   // | ------------------------- timers ------------------------- |
+     360             : 
+     361          42 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &Se3Controller::timerGains, this, false, false);
+     362             : 
+     363             :   // | ---------------------- position pid ---------------------- |
+     364             : 
+     365          42 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     366          42 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     367          42 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     368          42 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     369             : 
+     370             :   // | ------------------------ profiler ------------------------ |
+     371             : 
+     372          42 :   profiler_ = mrs_lib::Profiler(common_handlers_->parent_nh, "Se3Controller", _profiler_enabled_);
+     373             : 
+     374             :   // | ----------------------- finish init ---------------------- |
+     375             : 
+     376          42 :   ROS_INFO("[Se3Controller]: initialized");
+     377             : 
+     378          42 :   is_initialized_ = true;
+     379             : 
+     380          42 :   return true;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : /* //{ activate() */
+     386             : 
+     387          13 : bool Se3Controller::activate(const ControlOutput& last_control_output) {
+     388             : 
+     389          13 :   activation_control_output_ = last_control_output;
+     390             : 
+     391          13 :   double activation_mass = _uav_mass_;
+     392             : 
+     393          13 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     394           0 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     395           0 :     activation_mass += uav_mass_difference_;
+     396           0 :     ROS_INFO("[Se3Controller]: setting mass difference from the last control output: %.2f kg", uav_mass_difference_);
+     397             :   }
+     398             : 
+     399          13 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     400             : 
+     401          13 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     402           0 :     Ib_b_[0] = -activation_control_output_.diagnostics.disturbance_bx_b;
+     403           0 :     Ib_b_[1] = -activation_control_output_.diagnostics.disturbance_by_b;
+     404             : 
+     405           0 :     Iw_w_[0] = -activation_control_output_.diagnostics.disturbance_wx_w;
+     406           0 :     Iw_w_[1] = -activation_control_output_.diagnostics.disturbance_wy_w;
+     407             : 
+     408           0 :     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          13 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     416             : 
+     417             :   // rampup check
+     418          13 :   if (_rampup_enabled_ && throttle_last_controller) {
+     419             : 
+     420           8 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     421             : 
+     422           8 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     423             : 
+     424           8 :     if (throttle_difference > 0) {
+     425           2 :       rampup_direction_ = 1;
+     426           6 :     } else if (throttle_difference < 0) {
+     427           0 :       rampup_direction_ = -1;
+     428             :     } else {
+     429           6 :       rampup_direction_ = 0;
+     430             :     }
+     431             : 
+     432           8 :     ROS_INFO("[Se3Controller]: activating rampup with initial throttle: %.4f, target: %.4f", throttle_last_controller.value(), hover_throttle);
+     433             : 
+     434           8 :     rampup_active_     = true;
+     435           8 :     rampup_start_time_ = ros::Time::now();
+     436           8 :     rampup_last_time_  = ros::Time::now();
+     437           8 :     rampup_throttle_   = throttle_last_controller.value();
+     438             : 
+     439           8 :     rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
+     440             :   }
+     441             : 
+     442          13 :   first_iteration_ = true;
+     443          13 :   mute_gains_      = true;
+     444             : 
+     445          13 :   timer_gains_.start();
+     446             : 
+     447             :   // | ------------------ finish the activation ----------------- |
+     448             : 
+     449          13 :   ROS_INFO("[Se3Controller]: activated");
+     450             : 
+     451          13 :   is_active_ = true;
+     452             : 
+     453          13 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* //{ deactivate() */
+     459             : 
+     460           6 : void Se3Controller::deactivate(void) {
+     461             : 
+     462           6 :   is_active_           = false;
+     463           6 :   first_iteration_     = false;
+     464           6 :   uav_mass_difference_ = 0;
+     465             : 
+     466           6 :   timer_gains_.stop();
+     467             : 
+     468           6 :   ROS_INFO("[Se3Controller]: deactivated");
+     469           6 : }
+     470             : 
+     471             : //}
+     472             : 
+     473             : /* updateInactive() //{ */
+     474             : 
+     475       31457 : void Se3Controller::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command) {
+     476             : 
+     477       31457 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     478             : 
+     479       31457 :   last_update_time_ = uav_state.header.stamp;
+     480             : 
+     481       31457 :   first_iteration_ = false;
+     482       31457 : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ updateWhenActive() */
+     487             : 
+     488        9256 : Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+     489             : 
+     490       27768 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     491       27768 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     492             : 
+     493       18512 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     494             : 
+     495        9256 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     496             : 
+     497        9256 :   last_control_output_.desired_heading_rate          = {};
+     498        9256 :   last_control_output_.desired_orientation           = {};
+     499        9256 :   last_control_output_.desired_unbiased_acceleration = {};
+     500        9256 :   last_control_output_.control_output                = {};
+     501             : 
+     502             :   // | -------------------- calculate the dt -------------------- |
+     503             : 
+     504             :   double dt;
+     505             : 
+     506        9256 :   if (first_iteration_) {
+     507          13 :     dt               = 0.01;
+     508          13 :     first_iteration_ = false;
+     509             :   } else {
+     510        9243 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     511             :   }
+     512             : 
+     513        9256 :   last_update_time_ = uav_state.header.stamp;
+     514             : 
+     515        9256 :   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        9256 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     525             : 
+     526        9256 :   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        9256 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     536        4204 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude rate output");
+     537        4204 :     lowest_modality = common::ATTITUDE_RATE;
+     538        5052 :   } 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        5052 :   } 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        5052 :   } 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        9256 :   switch (lowest_modality.value()) {
+     550             : 
+     551         165 :     case common::POSITION: {
+     552         165 :       positionPassthrough(uav_state, tracker_command);
+     553         165 :       break;
+     554             :     }
+     555             : 
+     556         178 :     case common::VELOCITY_HDG: {
+     557         178 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     558         178 :       break;
+     559             :     }
+     560             : 
+     561         180 :     case common::VELOCITY_HDG_RATE: {
+     562         180 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     563         180 :       break;
+     564             :     }
+     565             : 
+     566         363 :     case common::ACCELERATION_HDG: {
+     567         363 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     568         363 :       break;
+     569             :     }
+     570             : 
+     571         361 :     case common::ACCELERATION_HDG_RATE: {
+     572         361 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     573         361 :       break;
+     574             :     }
+     575             : 
+     576         859 :     case common::ATTITUDE: {
+     577         859 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE);
+     578         859 :       break;
+     579             :     }
+     580             : 
+     581        4204 :     case common::ATTITUDE_RATE: {
+     582        4204 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     583        4204 :       break;
+     584             :     }
+     585             : 
+     586        1532 :     case common::CONTROL_GROUP: {
+     587        1532 :       SE3Controller(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     588        1532 :       break;
+     589             :     }
+     590             : 
+     591        1414 :     case common::ACTUATORS_CMD: {
+     592        1414 :       SE3Controller(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     593        1414 :       break;
+     594             :     }
+     595             : 
+     596        9256 :     default: {
+     597             :     }
+     598             :   }
+     599             : 
+     600        9256 :   return last_control_output_;
+     601             : }
+     602             : 
+     603             : //}
+     604             : 
+     605             : /* //{ getStatus() */
+     606             : 
+     607         739 : const mrs_msgs::ControllerStatus Se3Controller::getStatus() {
+     608             : 
+     609         739 :   mrs_msgs::ControllerStatus controller_status;
+     610             : 
+     611         739 :   controller_status.active = is_active_;
+     612             : 
+     613         739 :   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         162 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr Se3Controller::setConstraints([
+     674             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+     675             : 
+     676         162 :   if (!is_initialized_) {
+     677           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     678             :   }
+     679             : 
+     680         162 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     681             : 
+     682         162 :   ROS_INFO("[Se3Controller]: updating constraints");
+     683             : 
+     684         324 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     685         162 :   res.success = true;
+     686         162 :   res.message = "constraints updated";
+     687             : 
+     688         162 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     689             : }
+     690             : 
+     691             : //}
+     692             : 
+     693             : // --------------------------------------------------------------
+     694             : // |                         controllers                        |
+     695             : // --------------------------------------------------------------
+     696             : 
+     697             : /* SE3Controller() //{ */
+     698             : 
+     699        8733 : 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       17466 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     703        8733 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     704        8733 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     705             : 
+     706             :   // | ----------------- get the current heading ---------------- |
+     707             : 
+     708        8733 :   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        8733 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     719        8733 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     720        8733 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     721             : 
+     722        8733 :   if (tracker_command.use_position_vertical || tracker_command.use_position_horizontal) {
+     723             : 
+     724        8733 :     if (tracker_command.use_position_horizontal) {
+     725        8733 :       Rp[0] = tracker_command.position.x;
+     726        8733 :       Rp[1] = tracker_command.position.y;
+     727             :     } else {
+     728           0 :       Rv[0] = 0;
+     729           0 :       Rv[1] = 0;
+     730             :     }
+     731             : 
+     732        8733 :     if (tracker_command.use_position_vertical) {
+     733        8733 :       Rp[2] = tracker_command.position.z;
+     734             :     } else {
+     735           0 :       Rv[2] = 0;
+     736             :     }
+     737             :   }
+     738             : 
+     739        8733 :   if (tracker_command.use_velocity_horizontal) {
+     740        8733 :     Rv[0] = tracker_command.velocity.x;
+     741        8733 :     Rv[1] = tracker_command.velocity.y;
+     742             :   } else {
+     743           0 :     Rv[0] = 0;
+     744           0 :     Rv[1] = 0;
+     745             :   }
+     746             : 
+     747        8733 :   if (tracker_command.use_velocity_vertical) {
+     748        8733 :     Rv[2] = tracker_command.velocity.z;
+     749             :   } else {
+     750           0 :     Rv[2] = 0;
+     751             :   }
+     752             : 
+     753        8733 :   if (tracker_command.use_acceleration) {
+     754        8733 :     Ra << tracker_command.acceleration.x, tracker_command.acceleration.y, tracker_command.acceleration.z;
+     755             :   } else {
+     756           0 :     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        8733 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     764        8733 :   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        8733 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     768             : 
+     769             :   // Ow - UAV angular rate
+     770        8733 :   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        8733 :   Eigen::Vector3d Ep(0, 0, 0);
+     776             : 
+     777        8733 :   if (tracker_command.use_position_horizontal || tracker_command.use_position_vertical) {
+     778        8733 :     Ep = Rp - Op;
+     779             :   }
+     780             : 
+     781             :   // velocity control error
+     782        8733 :   Eigen::Vector3d Ev(0, 0, 0);
+     783             : 
+     784        8733 :   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        8733 :     Ev = Rv - Ov;
+     787             :   }
+     788             : 
+     789             :   // | --------------------- load the gains --------------------- |
+     790             : 
+     791        8733 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+     792             : 
+     793        8733 :   Eigen::Vector3d Ka(0, 0, 0);
+     794        8733 :   Eigen::Array3d  Kp(0, 0, 0);
+     795        8733 :   Eigen::Array3d  Kv(0, 0, 0);
+     796        8733 :   Eigen::Array3d  Kq(0, 0, 0);
+     797        8733 :   Eigen::Array3d  Kw(0, 0, 0);
+     798             : 
+     799             :   {
+     800        8733 :     std::scoped_lock lock(mutex_gains_);
+     801             : 
+     802        8733 :     if (tracker_command.use_position_horizontal) {
+     803        8733 :       Kp[0] = gains.kpxy;
+     804        8733 :       Kp[1] = gains.kpxy;
+     805             :     } else {
+     806           0 :       Kp[0] = 0;
+     807           0 :       Kp[1] = 0;
+     808             :     }
+     809             : 
+     810        8733 :     if (tracker_command.use_position_vertical) {
+     811        8733 :       Kp[2] = gains.kpz;
+     812             :     } else {
+     813           0 :       Kp[2] = 0;
+     814             :     }
+     815             : 
+     816        8733 :     if (tracker_command.use_velocity_horizontal) {
+     817        8733 :       Kv[0] = gains.kvxy;
+     818        8733 :       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        8733 :     if (tracker_command.use_velocity_vertical || tracker_command.use_position_vertical) {
+     826        8733 :       Kv[2] = gains.kvz;
+     827             :     } else {
+     828           0 :       Kv[2] = 0;
+     829             :     }
+     830             : 
+     831        8733 :     if (tracker_command.use_acceleration) {
+     832        8733 :       Ka << gains.kaxy, gains.kaxy, gains.kaz;
+     833             :     } else {
+     834           0 :       Ka << 0, 0, 0;
+     835             :     }
+     836             : 
+     837        8733 :     if (!tracker_command.use_attitude_rate) {
+     838        8733 :       Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+     839             :     }
+     840             : 
+     841        8733 :     Kw[0] = gains.kw_roll_pitch;
+     842        8733 :     Kw[1] = gains.kw_roll_pitch;
+     843        8733 :     Kw[2] = gains.kw_yaw;
+     844             :   }
+     845             : 
+     846        8733 :   Kp = Kp * (_uav_mass_ + uav_mass_difference_);
+     847        8733 :   Kv = Kv * (_uav_mass_ + uav_mass_difference_);
+     848             : 
+     849             :   // | --------------- desired orientation matrix --------------- |
+     850             : 
+     851             :   // get body integral in the world frame
+     852             : 
+     853        8733 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+     854             : 
+     855             :   {
+     856       17466 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+     857             : 
+     858        8733 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+     859        8733 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+     860        8733 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+     861        8733 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+     862        8733 :     Ib_b_stamped.vector.z        = 0;
+     863             : 
+     864       17466 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+     865             : 
+     866        8733 :     if (res) {
+     867        8733 :       Ib_w[0] = res.value().vector.x;
+     868        8733 :       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        8733 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+     877             : 
+     878        8733 :   Eigen::Vector3d feed_forward      = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+     879        8733 :   Eigen::Vector3d position_feedback = Kp * Ep.array();
+     880        8733 :   Eigen::Vector3d velocity_feedback = Kv * Ev.array();
+     881        8733 :   Eigen::Vector3d integral_feedback;
+     882             :   {
+     883        8733 :     std::scoped_lock lock(mutex_integrals_);
+     884             : 
+     885        8733 :     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       17466 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+     900             : 
+     901        8733 :     Eigen::Vector3d integration_switch(1, 1, 0);
+     902             : 
+     903             :     // integrate the world error
+     904        8733 :     if (tracker_command.use_position_horizontal) {
+     905        8733 :       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        8733 :     bool world_integral_saturated = false;
+     912        8733 :     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        8733 :     } else if (Iw_w_[0] > gains.kiwxy_lim) {
+     916           0 :       Iw_w_[0]                 = gains.kiwxy_lim;
+     917           0 :       world_integral_saturated = true;
+     918        8733 :     } 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        8733 :     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        8733 :     world_integral_saturated = false;
+     929        8733 :     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        8733 :     } else if (Iw_w_[1] > gains.kiwxy_lim) {
+     933           0 :       Iw_w_[1]                 = gains.kiwxy_lim;
+     934           0 :       world_integral_saturated = true;
+     935        8733 :     } 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        8733 :     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       17466 :     std::scoped_lock lock(mutex_gains_);
+     955             : 
+     956        8733 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+     957        8733 :     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       17466 :       geometry_msgs::Vector3Stamped Ep_stamped;
+     963             : 
+     964        8733 :       Ep_stamped.header.stamp    = ros::Time::now();
+     965        8733 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+     966        8733 :       Ep_stamped.vector.x        = Ep(0);
+     967        8733 :       Ep_stamped.vector.y        = Ep(1);
+     968        8733 :       Ep_stamped.vector.z        = Ep(2);
+     969             : 
+     970       26199 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+     971             : 
+     972        8733 :       if (res) {
+     973        8733 :         Ep_fcu_untilted[0] = res.value().vector.x;
+     974        8733 :         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       17466 :       geometry_msgs::Vector3Stamped Ev_stamped;
+     983             : 
+     984        8733 :       Ev_stamped.header.stamp    = ros::Time::now();
+     985        8733 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+     986        8733 :       Ev_stamped.vector.x        = Ev(0);
+     987        8733 :       Ev_stamped.vector.y        = Ev(1);
+     988        8733 :       Ev_stamped.vector.z        = Ev(2);
+     989             : 
+     990       26199 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+     991             : 
+     992        8733 :       if (res) {
+     993        8733 :         Ev_fcu_untilted[0] = res.value().vector.x;
+     994        8733 :         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        8733 :     if (tracker_command.use_position_horizontal) {
+    1002        8733 :       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        8733 :     bool body_integral_saturated = false;
+    1009        8733 :     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        8733 :     } else if (Ib_b_[0] > gains.kibxy_lim) {
+    1013           0 :       Ib_b_[0]                = gains.kibxy_lim;
+    1014           0 :       body_integral_saturated = true;
+    1015        8733 :     } 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        8733 :     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        8733 :     body_integral_saturated = false;
+    1026        8733 :     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        8733 :     } else if (Ib_b_[1] > gains.kibxy_lim) {
+    1030           0 :       Ib_b_[1]                = gains.kibxy_lim;
+    1031           0 :       body_integral_saturated = true;
+    1032        8733 :     } 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        8733 :     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        8733 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1045             : 
+    1046         724 :     Eigen::Vector3d des_acc = (position_feedback + velocity_feedback + integral_feedback) / total_mass + Ra;
+    1047             : 
+    1048         724 :     if (output_modality == common::ACCELERATION_HDG) {
+    1049             : 
+    1050         726 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1051             : 
+    1052         363 :       cmd.acceleration.x = des_acc[0];
+    1053         363 :       cmd.acceleration.y = des_acc[1];
+    1054         363 :       cmd.acceleration.z = des_acc[2];
+    1055             : 
+    1056         363 :       cmd.heading = tracker_command.heading;
+    1057             : 
+    1058         363 :       last_control_output_.control_output = cmd;
+    1059             : 
+    1060             :     } else {
+    1061             : 
+    1062         361 :       double des_hdg_ff = 0;
+    1063             : 
+    1064         361 :       if (tracker_command.use_heading_rate) {
+    1065         361 :         des_hdg_ff = tracker_command.heading_rate;
+    1066             :       }
+    1067             : 
+    1068         722 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1069             : 
+    1070         361 :       cmd.acceleration.x = des_acc[0];
+    1071         361 :       cmd.acceleration.y = des_acc[1];
+    1072         361 :       cmd.acceleration.z = des_acc[2];
+    1073             : 
+    1074         361 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1075             : 
+    1076         361 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1077             : 
+    1078         361 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1079             : 
+    1080         361 :       cmd.heading_rate = des_hdg_rate;
+    1081             : 
+    1082         361 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1083             : 
+    1084         361 :       last_control_output_.control_output = cmd;
+    1085             :     }
+    1086             : 
+    1087             :     // | -------------- unbiased desired acceleration ------------- |
+    1088             : 
+    1089         724 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1090             : 
+    1091             :     {
+    1092             : 
+    1093         724 :       Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1094             : 
+    1095        1448 :       geometry_msgs::Vector3Stamped world_accel;
+    1096             : 
+    1097         724 :       world_accel.header.stamp    = ros::Time::now();
+    1098         724 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1099         724 :       world_accel.vector.x        = unbiased_des_acc_world[0];
+    1100         724 :       world_accel.vector.y        = unbiased_des_acc_world[1];
+    1101         724 :       world_accel.vector.z        = unbiased_des_acc_world[2];
+    1102             : 
+    1103        2172 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1104             : 
+    1105         724 :       if (res) {
+    1106         724 :         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         724 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1112             : 
+    1113             :     // | ----------------- fill in the diagnostics ---------------- |
+    1114             : 
+    1115         724 :     last_control_output_.diagnostics.ramping_up = false;
+    1116             : 
+    1117         724 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1118         724 :     last_control_output_.diagnostics.mass_difference = 0;
+    1119         724 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1120             : 
+    1121         724 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1122             : 
+    1123         724 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1124         724 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1125             : 
+    1126         724 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1127         724 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1128             : 
+    1129         724 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1130         724 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1131             : 
+    1132         724 :     last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1133             : 
+    1134         724 :     last_control_output_.diagnostics.controller = "Se3Controller";
+    1135             : 
+    1136         724 :     return;
+    1137             :   }
+    1138             : 
+    1139             :   /* mass estimatior //{ */
+    1140             : 
+    1141             :   // --------------------------------------------------------------
+    1142             :   // |                integrate the mass difference               |
+    1143             :   // --------------------------------------------------------------
+    1144             : 
+    1145             :   {
+    1146       16018 :     std::scoped_lock lock(mutex_gains_);
+    1147             : 
+    1148        8009 :     if (tracker_command.use_position_vertical && !rampup_active_) {
+    1149        8001 :       uav_mass_difference_ += gains.km * Ep[2] * dt;
+    1150             :     }
+    1151             : 
+    1152             :     // saturate the mass estimator
+    1153        8009 :     bool uav_mass_saturated = false;
+    1154        8009 :     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        8009 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1158           0 :       uav_mass_difference_ = gains.km_lim;
+    1159           0 :       uav_mass_saturated   = true;
+    1160        8009 :     } 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        8009 :     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        8009 :   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        8009 :   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        8009 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1188             : 
+    1189        8009 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "Se3Controller");
+    1190             : 
+    1191        8009 :   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        8009 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1205             : 
+    1206             :   // --------------------------------------------------------------
+    1207             :   // |               desired orientation + throttle               |
+    1208             :   // --------------------------------------------------------------
+    1209             : 
+    1210             :   // | ------------------- desired orientation ------------------ |
+    1211             : 
+    1212        8009 :   Eigen::Matrix3d Rd;
+    1213             : 
+    1214        8009 :   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        8009 :     Eigen::Vector3d bxd;  // desired heading vector
+    1231             : 
+    1232        8009 :     if (tracker_command.use_heading) {
+    1233        8009 :       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        8009 :     Rd = common::so3transform(f_normed, bxd, drs_params.rotation_type == 1);
+    1240             :   }
+    1241             : 
+    1242             :   // | -------------------- desired throttle -------------------- |
+    1243             : 
+    1244        8009 :   double desired_thrust_force = f.dot(R.col(2));
+    1245        8009 :   double throttle             = 0;
+    1246             : 
+    1247        8009 :   if (tracker_command.use_throttle) {
+    1248             : 
+    1249             :     // the throttle is overriden from the tracker command
+    1250           0 :     throttle = tracker_command.throttle;
+    1251             : 
+    1252        8009 :   } else if (rampup_active_) {
+    1253             : 
+    1254             :     // deactivate the rampup when the times up
+    1255           8 :     if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1256             : 
+    1257           8 :       rampup_active_ = false;
+    1258             : 
+    1259           8 :       ROS_INFO("[Se3Controller]: rampup finished");
+    1260             : 
+    1261             :     } else {
+    1262             : 
+    1263           0 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1264             : 
+    1265           0 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1266             : 
+    1267           0 :       rampup_last_time_ = ros::Time::now();
+    1268             : 
+    1269           0 :       throttle = rampup_throttle_;
+    1270             : 
+    1271           0 :       ROS_INFO_THROTTLE(0.1, "[Se3Controller]: ramping up throttle, %.4f", throttle);
+    1272             :     }
+    1273             : 
+    1274             :   } else {
+    1275             : 
+    1276        8001 :     if (desired_thrust_force >= 0) {
+    1277        8001 :       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        8009 :   bool throttle_saturated = false;
+    1286             : 
+    1287        8009 :   if (!std::isfinite(throttle)) {
+    1288             : 
+    1289           0 :     ROS_ERROR("[Se3Controller]: NaN detected in variable 'throttle'!!!");
+    1290           0 :     return;
+    1291             : 
+    1292        8009 :   } 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        8009 :   } 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        8009 :   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        8009 :   Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1321             : 
+    1322             :   {
+    1323        8009 :     Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1324             : 
+    1325       16018 :     geometry_msgs::Vector3Stamped world_accel;
+    1326             : 
+    1327        8009 :     world_accel.header.stamp    = ros::Time::now();
+    1328        8009 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1329        8009 :     world_accel.vector.x        = unbiased_des_acc_world[0];
+    1330        8009 :     world_accel.vector.y        = unbiased_des_acc_world[1];
+    1331        8009 :     world_accel.vector.z        = unbiased_des_acc_world[2];
+    1332             : 
+    1333       24027 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1334             : 
+    1335        8009 :     if (res) {
+    1336        8009 :       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        8009 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1344             : 
+    1345             :   // fill the unbiased desired accelerations
+    1346        8009 :   last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1347             : 
+    1348             :   // | ----------------- fill in the diagnostics ---------------- |
+    1349             : 
+    1350        8009 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1351             : 
+    1352        8009 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1353        8009 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1354        8009 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1355             : 
+    1356        8009 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1357             : 
+    1358        8009 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1359        8009 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1360             : 
+    1361        8009 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1362        8009 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1363             : 
+    1364        8009 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1365        8009 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1366             : 
+    1367        8009 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1368             : 
+    1369        8009 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1370             : 
+    1371             :   // | ------------ construct the attitude reference ------------ |
+    1372             : 
+    1373        8009 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1374             : 
+    1375        8009 :   attitude_cmd.stamp       = ros::Time::now();
+    1376        8009 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1377        8009 :   attitude_cmd.throttle    = throttle;
+    1378             : 
+    1379        8009 :   if (output_modality == common::ATTITUDE) {
+    1380             : 
+    1381         859 :     last_control_output_.control_output = attitude_cmd;
+    1382             : 
+    1383         859 :     return;
+    1384             :   }
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                      attitude control                      |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390        7150 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1391             : 
+    1392        7150 :   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        7150 :   } else if (tracker_command.use_heading_rate) {
+    1397             : 
+    1398             :     // to fill in the feed forward yaw rate
+    1399        7150 :     double desired_yaw_rate = 0;
+    1400             : 
+    1401             :     try {
+    1402        7150 :       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        7150 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1409             :   }
+    1410             : 
+    1411             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1412             : 
+    1413        7150 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1414             : 
+    1415        7150 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1416             : 
+    1417        7139 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: using jerk feedforward");
+    1418             : 
+    1419        7139 :     Eigen::Matrix3d I;
+    1420        7139 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1421        7139 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1422        7139 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1423             :   }
+    1424             : 
+    1425             :   // | --------------- run the attitude controller -------------- |
+    1426             : 
+    1427        7150 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1428             : 
+    1429        7150 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq,
+    1430       14300 :                                                           drs_params.pitch_roll_heading_rate_compensation);
+    1431             : 
+    1432        7150 :   if (!attitude_rate_command) {
+    1433           0 :     return;
+    1434             :   }
+    1435             : 
+    1436             :   // | --------- fill in the already known attitude rate -------- |
+    1437             : 
+    1438             :   {
+    1439             :     try {
+    1440        7150 :       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        7150 :   if (output_modality == common::ATTITUDE_RATE) {
+    1449             : 
+    1450        4204 :     last_control_output_.control_output = attitude_rate_command;
+    1451             : 
+    1452        4204 :     return;
+    1453             :   }
+    1454             : 
+    1455             :   // --------------------------------------------------------------
+    1456             :   // |                    Attitude rate control                   |
+    1457             :   // --------------------------------------------------------------
+    1458             : 
+    1459        2946 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1460             : 
+    1461        2946 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1462             : 
+    1463        2946 :   if (!control_group_command) {
+    1464           0 :     return;
+    1465             :   }
+    1466             : 
+    1467        2946 :   if (output_modality == common::CONTROL_GROUP) {
+    1468             : 
+    1469        1532 :     last_control_output_.control_output = control_group_command;
+    1470             : 
+    1471        1532 :     return;
+    1472             :   }
+    1473             : 
+    1474             :   // --------------------------------------------------------------
+    1475             :   // |                        output mixer                        |
+    1476             :   // --------------------------------------------------------------
+    1477             : 
+    1478        2828 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1479             : 
+    1480        1414 :   last_control_output_.control_output = actuator_cmd;
+    1481             : 
+    1482        1414 :   return;
+    1483             : }
+    1484             : 
+    1485             : //}
+    1486             : 
+    1487             : /* positionPassthrough() //{ */
+    1488             : 
+    1489         165 : void Se3Controller::positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1490             : 
+    1491         165 :   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         330 :   mrs_msgs::HwApiPositionCmd cmd;
+    1497             : 
+    1498         165 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1499         165 :   cmd.header.stamp    = ros::Time::now();
+    1500             : 
+    1501         165 :   cmd.position = tracker_command.position;
+    1502         165 :   cmd.heading  = tracker_command.heading;
+    1503             : 
+    1504         165 :   last_control_output_.control_output = cmd;
+    1505             : 
+    1506             :   // fill the unbiased desired accelerations
+    1507         165 :   last_control_output_.desired_unbiased_acceleration = {};
+    1508         165 :   last_control_output_.desired_orientation           = {};
+    1509         165 :   last_control_output_.desired_heading_rate          = {};
+    1510             : 
+    1511             :   // | ----------------- fill in the diagnostics ---------------- |
+    1512             : 
+    1513         165 :   last_control_output_.diagnostics.ramping_up = false;
+    1514             : 
+    1515         165 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1516         165 :   last_control_output_.diagnostics.mass_difference = 0;
+    1517             : 
+    1518         165 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1519             : 
+    1520         165 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1521         165 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1522             : 
+    1523         165 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1524         165 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1525             : 
+    1526         165 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1527         165 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1528             : 
+    1529         165 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1530             : 
+    1531         165 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1532             : }
+    1533             : 
+    1534             : //}
+    1535             : 
+    1536             : /* PIDVelocityOutput() //{ */
+    1537             : 
+    1538         358 : 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         358 :   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         358 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1547         358 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1548             : 
+    1549         358 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1550         358 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1551             : 
+    1552         358 :   double hdg_ref = tracker_command.heading;
+    1553         358 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1554             : 
+    1555             :   // | ------------------ velocity feedforward ------------------ |
+    1556             : 
+    1557         358 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1558             : 
+    1559         358 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1560         358 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1561             :   }
+    1562             : 
+    1563             :   // | -------------------------- gains ------------------------- |
+    1564             : 
+    1565         358 :   Eigen::Vector3d Kp;
+    1566             : 
+    1567             :   {
+    1568         358 :     std::scoped_lock lock(mutex_gains_);
+    1569             : 
+    1570         358 :     Kp << gains.kpxy, gains.kpxy, gains.kpz;
+    1571             :   }
+    1572             : 
+    1573             :   // | --------------------- control errors --------------------- |
+    1574             : 
+    1575         358 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1576             : 
+    1577             :   // | --------------------------- pid -------------------------- |
+    1578             : 
+    1579         358 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1580         358 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1581         358 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1582             : 
+    1583         358 :   double des_vel_x = position_pid_x_.update(Ep[0], dt);
+    1584         358 :   double des_vel_y = position_pid_y_.update(Ep[1], dt);
+    1585         358 :   double des_vel_z = position_pid_z_.update(Ep[2], dt);
+    1586             : 
+    1587             :   // | -------------------- position feedback ------------------- |
+    1588             : 
+    1589         358 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1590             : 
+    1591         358 :   if (control_output == common::VELOCITY_HDG) {
+    1592             : 
+    1593             :     // | --------------------- fill the output -------------------- |
+    1594             : 
+    1595         356 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1596             : 
+    1597         178 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1598         178 :     cmd.header.stamp    = ros::Time::now();
+    1599             : 
+    1600         178 :     cmd.velocity.x = des_vel[0];
+    1601         178 :     cmd.velocity.y = des_vel[1];
+    1602         178 :     cmd.velocity.z = des_vel[2];
+    1603             : 
+    1604         178 :     cmd.heading = tracker_command.heading;
+    1605             : 
+    1606         178 :     last_control_output_.control_output = cmd;
+    1607             : 
+    1608         180 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1609             : 
+    1610         180 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1611             : 
+    1612         180 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1613             : 
+    1614         180 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1615             : 
+    1616             :     // | --------------------------- ff --------------------------- |
+    1617             : 
+    1618         180 :     double des_hdg_ff = 0;
+    1619             : 
+    1620         180 :     if (tracker_command.use_heading_rate) {
+    1621         180 :       des_hdg_ff = tracker_command.heading_rate;
+    1622             :     }
+    1623             : 
+    1624             :     // | --------------------- fill the output -------------------- |
+    1625             : 
+    1626         360 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1627             : 
+    1628         180 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1629         180 :     cmd.header.stamp    = ros::Time::now();
+    1630             : 
+    1631         180 :     cmd.velocity.x = des_vel[0];
+    1632         180 :     cmd.velocity.y = des_vel[1];
+    1633         180 :     cmd.velocity.z = des_vel[2];
+    1634             : 
+    1635         180 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1636             : 
+    1637         180 :     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         358 :   last_control_output_.desired_unbiased_acceleration = {};
+    1646         358 :   last_control_output_.desired_orientation           = {};
+    1647         358 :   last_control_output_.desired_heading_rate          = {};
+    1648             : 
+    1649             :   // | ----------------- fill in the diagnostics ---------------- |
+    1650             : 
+    1651         358 :   last_control_output_.diagnostics.ramping_up = false;
+    1652             : 
+    1653         358 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1654         358 :   last_control_output_.diagnostics.mass_difference = 0;
+    1655             : 
+    1656         358 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1657             : 
+    1658         358 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1659         358 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1660             : 
+    1661         358 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1662         358 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1663             : 
+    1664         358 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1665         358 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1666             : 
+    1667         358 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1668             : 
+    1669         358 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1670             : }
+    1671             : 
+    1672             : //}
+    1673             : 
+    1674             : // --------------------------------------------------------------
+    1675             : // |                          callbacks                         |
+    1676             : 
+    1677             : 
+    1678             : /* //{ callbackDrs() */
+    1679             : 
+    1680          84 : void Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, [[maybe_unused]] uint32_t level) {
+    1681             : 
+    1682          84 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1683             : 
+    1684          84 :   ROS_INFO("[Se3Controller]: DRS updated gains");
+    1685          84 : }
+    1686             : 
+    1687             : //}
+    1688             : 
+    1689             : // --------------------------------------------------------------
+    1690             : // |                           timers                           |
+    1691             : // --------------------------------------------------------------
+    1692             : 
+    1693             : /* timerGains() //{ */
+    1694             : 
+    1695        1140 : void Se3Controller::timerGains(const ros::TimerEvent& event) {
+    1696             : 
+    1697        3420 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1698             :   mrs_lib::ScopeTimer timer =
+    1699        3420 :       mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1700             : 
+    1701        2280 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1702        1140 :   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        1140 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    1707        1140 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    1708             : 
+    1709        1140 :   mute_gains_ = false;
+    1710             : 
+    1711        1140 :   const double dt = (event.current_real - event.last_real).toSec();
+    1712             : 
+    1713        1140 :   bool updated = false;
+    1714             : 
+    1715        1140 :   gains.kpxy          = calculateGainChange(dt, gains.kpxy, drs_params.kpxy * gain_coeff, bypass_filter, "kpxy", updated);
+    1716        1140 :   gains.kvxy          = calculateGainChange(dt, gains.kvxy, drs_params.kvxy * gain_coeff, bypass_filter, "kvxy", updated);
+    1717        1140 :   gains.kaxy          = calculateGainChange(dt, gains.kaxy, drs_params.kaxy * gain_coeff, bypass_filter, "kaxy", updated);
+    1718        1140 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    1719        1140 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    1720        1140 :   gains.kpz           = calculateGainChange(dt, gains.kpz, drs_params.kpz * gain_coeff, bypass_filter, "kpz", updated);
+    1721        1140 :   gains.kvz           = calculateGainChange(dt, gains.kvz, drs_params.kvz * gain_coeff, bypass_filter, "kvz", updated);
+    1722        1140 :   gains.kaz           = calculateGainChange(dt, gains.kaz, drs_params.kaz * gain_coeff, bypass_filter, "kaz", updated);
+    1723        1140 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    1724        1140 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    1725        1140 :   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        1140 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    1729        1140 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    1730        1140 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    1731             : 
+    1732        1140 :   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        1140 :   if (updated) {
+    1737             : 
+    1738         117 :     drs_params.kpxy          = gains.kpxy;
+    1739         117 :     drs_params.kvxy          = gains.kvxy;
+    1740         117 :     drs_params.kaxy          = gains.kaxy;
+    1741         117 :     drs_params.kiwxy         = gains.kiwxy;
+    1742         117 :     drs_params.kibxy         = gains.kibxy;
+    1743         117 :     drs_params.kpz           = gains.kpz;
+    1744         117 :     drs_params.kvz           = gains.kvz;
+    1745         117 :     drs_params.kaz           = gains.kaz;
+    1746         117 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    1747         117 :     drs_params.kq_yaw        = gains.kq_yaw;
+    1748         117 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    1749         117 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    1750         117 :     drs_params.km            = gains.km;
+    1751         117 :     drs_params.km_lim        = gains.km_lim;
+    1752             : 
+    1753         117 :     drs_->updateConfig(drs_params);
+    1754             : 
+    1755         117 :     ROS_INFO_THROTTLE(10.0, "[Se3Controller]: gains have been updated");
+    1756             :   }
+    1757        1140 : }
+    1758             : 
+    1759             : //}
+    1760             : 
+    1761             : // --------------------------------------------------------------
+    1762             : // |                       other routines                       |
+    1763             : // --------------------------------------------------------------
+    1764             : 
+    1765             : /* calculateGainChange() //{ */
+    1766             : 
+    1767       15960 : 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       15960 :   double change = desired_value - current_value;
+    1771             : 
+    1772       15960 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    1773       15960 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    1774             : 
+    1775       15960 :   if (!bypass_rate) {
+    1776             : 
+    1777             :     // if current value is near 0...
+    1778             :     double change_in_perc;
+    1779             :     double saturated_change;
+    1780             : 
+    1781       15817 :     if (fabs(current_value) < 1e-6) {
+    1782           0 :       change *= gains_filter_max_change;
+    1783             :     } else {
+    1784             : 
+    1785       15817 :       saturated_change = change;
+    1786             : 
+    1787       15817 :       change_in_perc = (current_value + saturated_change) / current_value - 1.0;
+    1788             : 
+    1789       15817 :       if (change_in_perc > gains_filter_max_change) {
+    1790        1001 :         saturated_change = current_value * gains_filter_max_change;
+    1791       14816 :       } else if (change_in_perc < -gains_filter_max_change) {
+    1792          22 :         saturated_change = current_value * -gains_filter_max_change;
+    1793             :       }
+    1794             : 
+    1795       15817 :       if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
+    1796          13 :         change *= gains_filter_min_change;
+    1797             :       } else {
+    1798       15804 :         change = saturated_change;
+    1799             :       }
+    1800             :     }
+    1801             :   }
+    1802             : 
+    1803       15960 :   if (fabs(change) > 1e-3) {
+    1804        1309 :     ROS_DEBUG("[Se3Controller]: changing gain '%s' from %.2f to %.2f", name.c_str(), current_value, desired_value);
+    1805        1309 :     updated = true;
+    1806             :   }
+    1807             : 
+    1808       15960 :   return current_value + change;
+    1809             : }
+    1810             : 
+    1811             : //}
+    1812             : 
+    1813             : /* getHeadingSafely() //{ */
+    1814             : 
+    1815        9091 : double Se3Controller::getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1816             : 
+    1817             :   try {
+    1818        9091 :     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          42 : 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..afb671c5d34ce6ca6b223aeabfae4877a52e212a GIT binary patch literal 5820 zcmV;t7DMTYP)YR;5o}f-jrz-}$ z1i(om+7m+nnZAkB-t4LtIF2!j>}D`dYzHQ76anYrIJK4mQ}g77`4H_M82uDA&yk}| zQ*eHo#gM+fxZW^Y211=Hrbc?<2^H6%VTtJ_L`Xj(Pf-eNRAXEde)2Pc2_sn81I8A? zBtl3DuRzXdt_|%wG0qet!&5^;^=*pGsB1ch(KmUTvW*4{S02z%b*VVclmX3xJ5A`< zGw#!hsgFoN0dRllHep0!rcnRXq)NjsNWYlTXLc;i^v4q5g;s&S#9z$hbGh-?AU%Q( zQOA8OJrnp_&u}zJ2{Uum5|>4YF-~5B8uz5MI<8_=^(3^`UiE;UfyHY}5+L)tdR{tW zrcm#6DzE-s6)`Hi8BnjQx`D?yH*tf*7+QJp`!4rq_PXRu0NsAU&-jyvY^pj3c!?7| zP;={fN(8zV4a7*s4A}`ZjF}mpA+{swSE}r7UaGRQFv^C|&GE9yjM9si+^ z}!=_rBHR?O;#MPaHpFiG3qo$dDLP=a#e7~&6=v!1@*PF&o(O` zIx|#%z<;fcu(#g`D8lx&*B=;1*zB_^cQe(G3?g;t#b^o>|T(+hxG0Aw3tt$M|5>3}gyhY_Qn!{=+*>r6pc zxYo?!ppBA1teo^{sit^hPgyYXOnql8}t&$fYT(2Nm)2KKzG>i(Mjdx_B*bpC=+ zT*QEt4`VpprhZ@>iQe{+ZFJN8r?;`Jj()~Oy6byzl{xgkEdc5c!8y{pCA#ihpZx^#`<2tqS;fr>bBg#(UjzG&NS8{${i~bFNLuclHtLOn0jPLK~?! zWg^OEP0xs?i5uMXF;nxg$J;l1=Zi4oe#CaqZQAF?h#`(W8?gEKrwcbY;VS2!*(L6I zaBS!2!~J>h5*IY7(y9av8f(Tjb?jZ;Gv2k<16GWgPGMvVZj)e~{|BKnZFFTp%%wq*t|ZFFRn0ZmBVS1|nA8KASE`C5*eqztJDL z8rGkGl#iJ|Y4FF;MLl)zPIry_DX7z9=;LpsE z_yOD{UNN{VZPc+xT^2_1*^Y>54dRomvp~?0j%eee!V%dD zO?FDaqK(wnMrjUf#CXf@UDaZyu6kf(YPZ~6T)5tfpN@5|&Dj+3#D1;7h*ToZglxu` zmG8eJ6kz%2fl(3%`l2QtSI`+;k3#dTtY5Fjs@Ft#KqX$5K0?9?JB|ss2)Jkg&z6<| zfs+aWYKciAQ;*pSysA7P(nGR+0UIQC9|Nd{{*gRjWN>Cqxg0ww4IlSi92_H3Ncl+h zy5o$y2Hx~B_$J zheznr-8g2n#AOY$gS&7Fr^mxzmjSFxga$If_-AaV*vP|rKCTE5jccw8iMhvP3?k*e z%XHx@4#(FM*u$R@ie-4IYX4lW`kVk>T-B^-6szVVUAU^xKP=UJ)nmK>*eugJTo-7_ z%`9QiWapt-S#yA@sz=!N0DDAoQevVs01>%;5;5Wgts-WB`%%PX1eYfT6Z2U;BlL}y znA^t?W0(LqVj$rAiOHxmx}Mp-#C(a7h|z5L3{`ixAQ4qTuKG`z;=AS2PF6?w0Z3<~ zQJRgYtiqTHo~et*>0=|+cj3^K4t;FrXqy?!gFYGdt|_7i+(Jb_1I7~#)6^%}is&)A zO|G*`IHpc>$eL!~o;TaCy9RL3b3@@MYQ3GmCBuY2~ zF~f5Vby$;x1?feHX2oolmkAq2Jjn`Or3d^|ecjP(FGn6i?GzY%^FPs7t@Z*?Q#>uc zdw(kcdQCsHDD0k_6tE6sKr9`qDjk$SfPq2*(9*_??dn=2_yEkx`eaze)9GNyIK~l1(LI#>W!&!(JZZ( zTEK;8a^et?0O{w|rkhPcgHfbv5fVE1Z*3~qJo`jDIetr2Q+ck@NJHpsOo&vak%lCU zr4y6LRejHS0V8DSiD0mJk^&-Y9 z!2AR-;84|}nw@OE$!#?ND$9UGk(ZeIWeWRMGZM_6AXHJ=V>H>JdJiYpHC1Dbo0x?$ zshZ}_U;$%Z$3oRcK>5&sp6WG!-0WbkRew}P&n{%RWKTe-04*z5;?_ptILGrl&9fKi z3_2tC8tVvtF2_nK#c9>%%pplIucvJf10GG_LS7pHg#|>vDsWpVVHBD%p8D8H2zUz1 z@E{Y3#$OnH=YEZu(D#FV=RPL2PoPar#DK3It%n1tZrIwh!d@Yd=p0;XurRFGaF)s$ zuZ{acPX%|+J$J#*&LJ+fV>IW-uTxMGl| zycQmhCxeeOxgeojT#K32)?@TFguqX1nn-0c$c2_i1`7PEyneKc7RRg@ix)mbT^ z8e?)kb!&BBC5qH90@~V$S7$3 z<=WPz#xMkZQuNWXAPuq3%yg(S8@kL~k1UKfPX)-A5G|?I`4WUSCVS4BI4Oz~n_n{Q z!I6++8|ek$9op#orXxlirEdZ3RP|N?_?M|hnAE&4_h@uyT0nd=a4cf<#Ig}Zr0unr~jJ$(Q{@?dK*3&lsItL z5KKMc)wmhXk1*Bm9Q^1r{DFmHcUb7C(4QM}#}Y$hcIU*YuJd;clL%#q_JLVuH=SiBV4?gqp-_=M+& zmhw~$g?p)$XVC`&-OrGiV)dmMq^G|_@vSjP-Ls}cd+#Zm?mr^%EqgBcj_x#z0{wVi zRMULu)cyC2Dd0nx)C+1pq+TNR|#C&*!mk{x@q!mcrq)~_?!d-rr0xjVXH5T_rM zh7F{$T(}TTZoy%eI5G568m)k=BMBp_I=$yqU!tb)Vw)!T&|6XwHd%iP+Xtn z0xBX#LovRnb*2U*#aY7WW~!%zk+v`|6ySF~*h7659oU1L`7{tS(3sjO`iV&=!)Nj| zC~;=TVm_#;CR`aKZXiY()Ra^$1vO#7{l%{W8mLwTHH}M-bykoGYPuO*z9SW+5Zx5b zdbKI)uxW6Pei8`B#$6;GWi=sdBDDCbN&UVH)m8V6&=T4h24hXEt3dMxLZ7mL^eCp( zMjD~jp^d)wju=TERRdtBs<#Tjzf3j4+>S9d238DLknKM>-8&3dIo!R5>SYnnrYG!S zCQuG=6BAS#+sK@N;!XqVG4k>FXPfIWzAqdvTlm}Wyc~j8<9wQl<|)ukGP zMtVjaZdPjT;c_!=_V5s1<1bL*dIOxZ$sYYORjr$_mh?mpTl5;K()XFcT#e3p4_vS z{r{IS%Hv^$clkO@9xw)fu5QeXbf>~(It`Z+ab^lwD(n=Bo%X&%Az$kDg_mn$L><2`j3UYhl((31_$y&-7&V=9QTCvr z3x}kh4W-ZNy(HMtjY#}to6T4?IS~s!1Oddj`knhH#ISn_Zx6fxxf&(U6#2moS!*2#~U;@}d)1EZ(eW$$=ptX%2rN2PF14hJn22giQ(;$z+vB8%= z(0ROh99RCmGoIpW8J5e}GF*r8|7o86T85X8u?(|C2m`pF@AlF^ItsRN@>*UWjJ#W# z6x)$JP9$!@g4=w4W(|3Zl7(F@X6mXxN6aP~HZnA`+WA8^g-lgR#Vp1oLDi{3DDq+S zD~tokMQFv{hgiIS#?nT=krJ_O5pdyr3P{kcUVCRvoSjDF&w`T9p5cLSu?v6NxU7iL znOleazN@G5pY|gU%Y@3IM6vKi4kxE#4xm&+Ndk3O-j0Mv<_o(LM*y#^$yy9 zR}c3GhQO=wdiYU#MInn}$j83QPt<(RxG&+Psju3pg&SNTxxrB3M0N%1|JxRoxGZ1< zt?;`R((~IKIqo>>sjkaguWPhQ=2;E^yKLmhJryGLK0PKnAI=-4kGsyhhLMr|V0+Ij z)K$UUK5;6V7K|&w$w||JkqRvrDYgS5hbrF7YzB_IF3=^qss9T zFecG*97ZM8+S$1K$`!bH+*y>R8Zyw$-C-CDCOmmO+ATo?q$dyem6v~6hV_cO=s$*W~Ofe;4d?VG$3mfh!)k0H-v@uP@ z&tg>8Y^b{b1TatVDybeb0Kr(Ziy-9+wZdFOi$mW{K>q=XJb^`GL+jZ90000 + + + + + + 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:2023-12-06 07:59:45Functions: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..1e7cf82f8e --- /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:2023-12-06 07:59:45Functions: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..a22e4032fa --- /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:2023-12-06 07:59:45Functions: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..b5a9ad259d --- /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:2023-12-06 07:59:45Functions: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..8daaecadaa --- /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:2023-12-06 07:59:45Functions: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..dcb083c8ff --- /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:2023-12-06 07:59:45Functions: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..4688e8e53e --- /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:2023-12-06 07:59:45Functions: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::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)349
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24482
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)35422
+
+
+ + + +
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..f35acc71df --- /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:2023-12-06 07:59:45Functions: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*)35422
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)349
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24482
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
+
+
+ + + +
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..6d278a800b --- /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:2023-12-06 07:59:45Functions: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       35422 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52       35422 :     void operator()(const T& msg) {
+      53       35422 :       obj_->publish(msg);
+      54       35422 :     }
+      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:2023-12-06 07:59:45Functions: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&)35
mrs_uav_managers::AglEstimator::~AglEstimator().235
+
+
+ + + +
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..8475660038 --- /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:2023-12-06 07:59:45Functions: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&)35
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::~AglEstimator().235
+
+
+ + + +
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..404c69fa56 --- /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:2023-12-06 07:59:45Functions: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          35 :   AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      55          35 :       : Estimator(agl::type, name, frame_id), package_name_(package_name) {
+      56          35 :   }
+      57             : 
+      58          35 :   virtual ~AglEstimator(void) {
+      59          35 :   }
+      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..350aeca8c0 --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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::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::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::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)1
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)1
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)2
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&)342
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
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&)348
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
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&)349
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)349
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&)692
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
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&)737
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
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&)2507
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2507
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&)2771
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2771
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&)3190
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)3458
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)4585
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&)21026
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24538
+
+
+ + + +
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..9291d7a3d0 --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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&)2507
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&)3190
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&)348
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&)349
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&)21026
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&)2771
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&)737
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&)342
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&)692
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)2
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)1
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&)3458
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)1
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&)2507
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)4585
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)349
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24538
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2771
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
+
+
+ + + +
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..731c056292 --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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        2507 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83        2507 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87        2507 :     double throttle = 0;
+      88             : 
+      89       12535 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90       10028 :       throttle += msg.motors[i];
+      91             :     };
+      92             : 
+      93        2507 :     throttle /= msg.motors.size();
+      94             : 
+      95        2507 :     return throttle;
+      96             :   }
+      97        2771 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98        2771 :     return msg.throttle;
+      99             :   }
+     100        4585 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101        4585 :     return msg.throttle;
+     102             :   }
+     103       24538 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104       24538 :     return msg.throttle;
+     105             :   }
+     106         692 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107         692 :     return std::nullopt;
+     108             :   }
+     109         737 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110         737 :     return std::nullopt;
+     111             :   }
+     112         342 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113         342 :     return std::nullopt;
+     114             :   }
+     115         349 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116         349 :     return std::nullopt;
+     117             :   }
+     118         348 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119         348 :     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        2507 :   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        2507 :     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        2507 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153        2771 :   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        2771 :     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        2771 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        3190 :   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        3190 :     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        3190 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173       21026 :   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       21026 :     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       21026 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183         692 :   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         692 :     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         692 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193         737 :   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         737 :     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         737 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203         342 :   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         342 :     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         342 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213         349 :   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         349 :     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         349 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223         348 :   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         348 :     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         348 :     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           2 :   void operator()(mrs_msgs::HwApiActuatorCmd& msg, [[maybe_unused]] const mrs_msgs::UavState& uav_state, const double& min_throttle, const double& n_motors) {
+     255           2 :     initializeHwApiCmd(msg, min_throttle, n_motors);
+     256           2 :   }
+     257           1 :   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           1 :     initializeHwApiCmd(msg, min_throttle);
+     260           1 :   }
+     261           1 :   void operator()(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle, [[maybe_unused]] const double& n_motors) {
+     262           1 :     initializeHwApiCmd(msg, uav_state, min_throttle);
+     263           1 :   }
+     264        3458 :   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        3458 :     initializeHwApiCmd(msg, min_throttle);
+     267        3458 :   }
+     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..4a58fea6bfb8487eab0b036512243a1641ced171 GIT binary patch literal 918 zcmV;H18Mw;P)SG;o@xbzSFG+myC9aLN1(fNbk~ zo8~Z#JIU~~9<;xA$_nH^D(q4B>ekyik38zKtUYs*yC`crT=syeQGkHE_0*SUz1_hF5A{QRfck`DxcNU_dT$jii-35$#>KVy8@387!OEW7Dk)np=MF04 zk|Mq{@ema$s<0(dk)Gnw7(N0bfdt7RAkL0VCY5!;=z-;Y23S(TuE5i!Bxx`^eXiJJsQt^&>Fld$nSHOezy;bBQvvHaunK z`zb?d(?FTum@~*(filmWx$B&{p)x1Una#B=I4@F$QirF^dPikkuNsNkDWm*90+%U+ zSF~?dCer1rmBH&7SDEGCCCTC4`q(kX5V+-)8fP@e^B6Wb3W|p zPwAIl$uE%ndqDHDDf6jP0Brz-uON4*yAHe^&I92~D)?G42>d^UUjM0|xzM`n@??dGAFLlr4Ub + + + + + + 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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 27
<unnamed>64.6 %62 / 9681.5 %22 / 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..eb34f45180 --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 27
<unnamed>64.6 %62 / 9681.5 %22 / 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..f714ca9ee2 --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 27
<unnamed>64.6 %62 / 9681.5 %22 / 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..274612120f --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 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..87d6b27a6b --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 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..956cade0f1 --- /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:629664.6 %
Date:2023-12-06 07:59:45Functions:222781.5 %
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 +
64.6%64.6%
+ 		64.6 %62 / 9681.5 %22 / 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..88460dee7a --- /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:2023-12-06 07:59:45Functions: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().2210
+
+
+ + + +
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..b13ce1a3fa --- /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:2023-12-06 07:59:45Functions: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().2210
+
+
+ + + +
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..2dddc2cb3d --- /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:2023-12-06 07:59:45Functions: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         210 :   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:2023-12-06 07:59:45Functions: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&)229
mrs_uav_managers::Estimator::~Estimator().2229
+
+
+ + + +
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..59e20fea8d --- /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:2023-12-06 07:59:45Functions: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&)229
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::~Estimator().2229
+
+
+ + + +
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..2e03a7cb67 --- /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:2023-12-06 07:59:45Functions: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         229 :   Estimator(const std::string &type, const std::string &name, const std::string &frame_id) : type_(type), name_(name), frame_id_(frame_id) {
+      64         229 :   }
+      65             : 
+      66         229 :   virtual ~Estimator(void) {
+      67         229 :   }
+      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..b5b1d732c1 --- /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:8910981.7 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
<unnamed>81.0 %85 / 10586.7 %13 / 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..665c48083c --- /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:8910981.7 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
<unnamed>81.0 %85 / 10586.7 %13 / 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..413773d5fb --- /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:8910981.7 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
<unnamed>100.0 %4 / 466.7 %2 / 3
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 15
<unnamed>81.0 %85 / 10586.7 %13 / 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..54b7b08f8b --- /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:8910981.7 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 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..160898ad3b --- /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:8910981.7 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 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..072930190d --- /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:8910981.7 %
Date:2023-12-06 07:59:45Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
support.h +
81.0%81.0%
+ 		81.0 %85 / 10586.7 %13 / 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..ea08065633 --- /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:8510581.0 %
Date:2023-12-06 07:59:45Functions:131586.7 %
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&)0
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
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&)154
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)168
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)844
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)45105
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)92098
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)96014
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)98585
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)99873
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)102722
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)236118
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)240004
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)336242
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)471562
+
+
+ + + +
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..e81c768f50 --- /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:8510581.0 %
Date:2023-12-06 07:59:45Functions:131586.7 %
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&)96014
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)0
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)240004
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)336242
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)168
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)844
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)102722
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)45105
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)236118
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)92098
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)98585
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&)154
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)99873
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)471562
+
+
+ + + +
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..4d25eea2af --- /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:8510581.0 %
Date:2023-12-06 07:59:45Functions:131586.7 %
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         844 :   static std::string toSnakeCase(const std::string& str_in) {
+      39             : 
+      40         844 :     std::string str(1, tolower(str_in[0]));
+      41             : 
+      42       13044 :     for (auto it = str_in.begin() + 1; it != str_in.end(); ++it) {
+      43       12200 :       if (isupper(*it) && *(it - 1) != '_' && islower(*(it - 1))) {
+      44         686 :         str += "_";
+      45             :       }
+      46       12200 :       str += *it;
+      47             :     }
+      48             : 
+      49         844 :     std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+      50             : 
+      51         844 :     return str;
+      52             :   }
+      53             :   /*//}*/
+      54             : 
+      55             : /* toLowercase() //{ */
+      56             : 
+      57         168 : static std::string toLowercase(const std::string str_in) {
+      58         168 :   std::string str_out = str_in;
+      59         168 :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower);
+      60         168 :   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       92098 :   static tf2::Vector3 rotateVecByHdg(const geometry_msgs::Vector3& vec_in, const double hdg_in) {
+      99             : 
+     100       92098 :     const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+     101             : 
+     102       91508 :     const tf2::Vector3 vec_tf2(vec_in.x, vec_in.y, vec_in.z);
+     103             : 
+     104       91204 :     const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+     105             : 
+     106      182422 :     return vec_rotated;
+     107             :   }
+     108             :   //}
+     109             : 
+     110             :   /* noNans() //{ */
+     111      471562 :   static bool noNans(const geometry_msgs::TransformStamped& tf) {
+     112             : 
+     113     1414057 :     return (std::isfinite(tf.transform.rotation.x) && std::isfinite(tf.transform.rotation.y) && std::isfinite(tf.transform.rotation.z) &&
+     114     1885060 :             std::isfinite(tf.transform.rotation.w) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.y) &&
+     115      942709 :             std::isfinite(tf.transform.translation.z));
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* noNans() //{ */
+     120       99873 :   static bool noNans(const geometry_msgs::Quaternion& q) {
+     121             : 
+     122       99873 :     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      336242 :   static tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) {
+     136             : 
+     137      336242 :     tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z);
+     138             : 
+     139      336166 :     tf2::Quaternion q;
+     140      336125 :     tf2::fromMsg(pose_in.orientation, q);
+     141             : 
+     142      336073 :     tf2::Transform tf_out(q, position);
+     143             : 
+     144      671294 :     return tf_out;
+     145             :   }
+     146             : 
+     147             :   //}
+     148             : 
+     149             :   /* poseFromTf2() //{ */
+     150             : 
+     151      240004 :   static geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) {
+     152             : 
+     153      240004 :     geometry_msgs::Pose pose_out;
+     154      239523 :     pose_out.position.x = tf_in.getOrigin().getX();
+     155      239890 :     pose_out.position.y = tf_in.getOrigin().getY();
+     156      239960 :     pose_out.position.z = tf_in.getOrigin().getZ();
+     157             : 
+     158      239927 :     pose_out.orientation = tf2::toMsg(tf_in.getRotation());
+     159             : 
+     160      240089 :     return pose_out;
+     161             :   }
+     162             : 
+     163             :   //}
+     164             : 
+     165             :   /* msgFromTf2() //{ */
+     166       96014 :   static geometry_msgs::Transform msgFromTf2(const tf2::Transform& tf_in) {
+     167             : 
+     168       96014 :     geometry_msgs::Transform tf_out;
+     169       96014 :     tf_out.translation.x = tf_in.getOrigin().getX();
+     170       96014 :     tf_out.translation.y = tf_in.getOrigin().getY();
+     171       96014 :     tf_out.translation.z = tf_in.getOrigin().getZ();
+     172       96014 :     tf_out.rotation = tf2::toMsg(tf_in.getRotation());
+     173             : 
+     174       96014 :     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      236118 :   static geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) {
+     194             : 
+     195      236118 :     geometry_msgs::Vector3 vec_out;
+     196      236060 :     vec_out.x = point_in.x;
+     197      236060 :     vec_out.y = point_in.y;
+     198      236060 :     vec_out.z = point_in.z;
+     199             : 
+     200      236060 :     return vec_out;
+     201             :   }
+     202             : 
+     203             :   //}
+     204             : 
+     205             :   /*//{ rotateVector() */
+     206      102722 :   static geometry_msgs::Vector3 rotateVector(const geometry_msgs::Vector3& vec_in, const geometry_msgs::Quaternion& q_in) {
+     207             : 
+     208             :     try {
+     209      102722 :       Eigen::Matrix3d        R = mrs_lib::AttitudeConverter(q_in);
+     210      102715 :       Eigen::Vector3d        vec_in_eigen(vec_in.x, vec_in.y, vec_in.z);
+     211      102622 :       Eigen::Vector3d        vec_eigen_rotated = R * vec_in_eigen;
+     212      102671 :       geometry_msgs::Vector3 vec_out;
+     213      102506 :       vec_out.x = vec_eigen_rotated[0];
+     214      102452 :       vec_out.y = vec_eigen_rotated[1];
+     215      102558 :       vec_out.z = vec_eigen_rotated[2];
+     216      102571 :       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       98585 :   static nav_msgs::Odometry uavStateToOdom(const mrs_msgs::UavState& uav_state) {
+     227       98585 :     nav_msgs::Odometry odom;
+     228       98577 :     odom.header              = uav_state.header;
+     229       98581 :     odom.child_frame_id      = uav_state.child_frame_id;
+     230       98577 :     odom.pose.pose           = uav_state.pose;
+     231       98577 :     odom.twist.twist.angular = uav_state.velocity.angular;
+     232             : 
+     233       98577 :     tf2::Quaternion q;
+     234       98575 :     tf2::fromMsg(odom.pose.pose.orientation, q);
+     235       98581 :     odom.twist.twist.linear = Support::rotateVector(uav_state.velocity.linear, mrs_lib::AttitudeConverter(q.inverse()));
+     236             : 
+     237      196646 :     return odom;
+     238             :   }
+     239             :   /*//}*/
+     240             : 
+     241             :   /*//{ getPoseDiff() */
+     242           0 :   static geometry_msgs::Pose getPoseDiff(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) {
+     243             : 
+     244           0 :     tf2::Vector3 v1, v2;
+     245           0 :     tf2::fromMsg(p1.position, v1);
+     246           0 :     tf2::fromMsg(p2.position, v2);
+     247           0 :     const tf2::Vector3 v3 = v1 - v2;
+     248             : 
+     249           0 :     tf2::Quaternion q1, q2;
+     250           0 :     tf2::fromMsg(p1.orientation, q1);
+     251           0 :     tf2::fromMsg(p2.orientation, q2);
+     252           0 :     tf2::Quaternion q3 = q2 * q1.inverse();
+     253           0 :     q3.normalize();
+     254             : 
+     255           0 :     geometry_msgs::Pose pose_diff;
+     256           0 :     tf2::toMsg(v3, pose_diff.position);
+     257           0 :     pose_diff.orientation = tf2::toMsg(q3);
+     258             : 
+     259           0 :     return pose_diff;
+     260             :   }
+     261             :   /*//}*/
+     262             : 
+     263             :   /*//{ applyPoseDiff() */
+     264       45105 :   static geometry_msgs::Pose applyPoseDiff(const geometry_msgs::Pose& pose_in, const geometry_msgs::Pose& pose_diff) {
+     265             : 
+     266       45105 :     tf2::Vector3    pos_in;
+     267       45105 :     tf2::Quaternion q_in;
+     268       45105 :     tf2::fromMsg(pose_in.position, pos_in);
+     269       45105 :     tf2::fromMsg(pose_in.orientation, q_in);
+     270             : 
+     271       45105 :     tf2::Vector3    pos_diff;
+     272       45105 :     tf2::Quaternion q_diff;
+     273       45105 :     tf2::fromMsg(pose_diff.position, pos_diff);
+     274       45105 :     tf2::fromMsg(pose_diff.orientation, q_diff);
+     275             : 
+     276       45105 :     const tf2::Vector3    pos_out = tf2::quatRotate(q_diff.inverse(), (pos_in - pos_diff));
+     277       45105 :     const tf2::Quaternion q_out   = q_diff.inverse() * q_in;
+     278             : 
+     279       45105 :     geometry_msgs::Pose pose_out;
+     280       45105 :     tf2::toMsg(pos_out, pose_out.position);
+     281       45105 :     pose_out.orientation = tf2::toMsg(q_out);
+     282             : 
+     283       90210 :     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         154 :   static bool isStringInVector(const std::string& value, const std::vector<std::string>& str_vec) {
+     300         154 :     return std::find(str_vec.begin(), str_vec.end(), value) != str_vec.end();
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /*//{ frameIdToEstimatorName() */
+     305           0 :   static std::string frameIdToEstimatorName(const std::string& str_in) {
+     306           0 :     const std::string str_tmp = str_in.substr(str_in.find("/")+1, str_in.size());
+     307           0 :     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..31acc42b914f3c4b0b1957dfbad8fa858a556f1a GIT binary patch literal 1253 zcmVV2m}&=1dIKgVcv9Xd%cj5pTK*JdrX!l0D0q8!rKtManR~C_iC*jqE*)WvMB!L)YER3m8-H zssX$AjJvM&eMi^I_05FtWUL>ACNHmnU-almjzzoLfcdzkpj>noC}-4`aQ1xKT{>%v@d~EKR50t$+KAAzDqiai;8y6iT14+6T z5L%#!aFS28S2(DeOd=@1aSC}^3{WP^bc54`xIBzQC*qPP*w;A~CK|ZbdD})nIyJF_ zFg@Uftn~=>-C#X8B&6;$16dfNVSGDKWC-|mi^JI;BJP0RtJpUy$qgy)rmPu6txS)TP9j6`4YK;$v}`-|MfCcp?t*gt%``9Ykxwmx08Gn!k4z@)RB zEnnz5mt3whS|S!G_CmS!{FFT|G`G9IF4bI^LX^fCAj&NI4ZwC`P=P{OuVe)d_(S%{ z=2u#K!0Zr1yEZd52d_-w`+&3pC&+&u&G_mpmK#ukpz>pAU;0mIM8I>4BXIkuMIc(w zY8)d!zQ=DYN+r+2eYSH+NXb7tH$OHbl7bI;phnB?Zv)C&j1=tN$4^9xxO7|PE*b?{ zPm$}d0cYSoGuACP0Mm3-EU#|?@&+m2IeR|$eKY^I zXT2GqK|Yl^W_&C=p5ZD`bZ=)xafF4|~ zr6LeB7P|&lSqhYIc^Y<_ssqoZA>7#>?0Mklc5=mM6@m}Cww^AG&sw+NA0iiY?iwi8 zs%Jk?ps>Ak0s4ruZri?=2GB0dp@vErB4f1D=g}8{R*=n$mBA)JRv1}J$LRWHfr-6v zjC+u!AWX{`S6=LgFnc`j%?folEG!JAhkdRDsUr$6l498PP>WaRh+8NxvI~{DXn9eX z$|#XAGO4kuPq*aqj9xP0T-u9slXJ>R?i9>vV{wY>I4JwvyjmEq>cAxbpOOD|$xavm z=#CE@8W`bFW~eOTd*#oB*kcCAx3(p{IBD#X2018BrF3V0B)du}$?@aPHTYvK$Hg%M zsUE+hV6Rr}Xsa?tif|SaY(BH)n;cDpQvmu)%0fo)SQf_29?OB_!xhr-248LXc`x3u zC;6JPt|&o&GGC99^*=z0Rw|?e6tB|tw+S_bK;m0>#@EMaNyvXM$ZFvaK;NVg$O{%g P00000NkvXXu0mjfEiFX= literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html new file mode 100644 index 0000000000..0c41074400 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.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:2023-12-06 07:59:45Functions: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
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html new file mode 100644 index 0000000000..f6a83ae9cf --- /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:2023-12-06 07:59:45Functions: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
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html new file mode 100644 index 0000000000..a3cd151468 --- /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:2023-12-06 07:59:45Functions: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..6956868382 --- /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:2023-12-06 07:59:45Functions: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
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html new file mode 100644 index 0000000000..0923d8ec3e --- /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:2023-12-06 07:59:45Functions: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
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index.html b/mrs_uav_managers/include/mrs_uav_managers/index.html new file mode 100644 index 0000000000..cfcc00315a --- /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:2023-12-06 07:59:45Functions: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..fe89391614 --- /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:2023-12-06 07:59:45Functions: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&)42
mrs_uav_managers::StateEstimator::~StateEstimator().242
+
+
+ + + +
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..864cdf0759 --- /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:2023-12-06 07:59:45Functions: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&)42
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::~StateEstimator().242
+
+
+ + + +
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..09b1a145eb --- /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:2023-12-06 07:59:45Functions: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          42 :   StateEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      62          42 :       : Estimator(state::type, name, frame_id), package_name_(package_name) {
+      63          42 :   }
+      64             : 
+      65          42 :   virtual ~StateEstimator(void) {
+      66          42 :   }
+      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..ee50b99dee --- /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:2023-12-06 07:59:45Functions: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().2253
+
+
+ + + +
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..730c60f402 --- /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:2023-12-06 07:59:45Functions: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().2253
+
+
+ + + +
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..5b7b78a850 --- /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:2023-12-06 07:59:45Functions: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         253 :   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:18639547.1 %
Date:2023-12-06 07:59:45Functions:111957.9 %
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 +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
<unnamed>78.5 %186 / 23778.6 %11 / 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..cbe85b5112 --- /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:18639547.1 %
Date:2023-12-06 07:59:45Functions:111957.9 %
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 +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
<unnamed>78.5 %186 / 23778.6 %11 / 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..7c49d7a16b --- /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:18639547.1 %
Date:2023-12-06 07:59:45Functions:111957.9 %
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 +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 14
<unnamed>78.5 %186 / 23778.6 %11 / 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..14789e7eee --- /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:18639547.1 %
Date:2023-12-06 07:59:45Functions:111957.9 %
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 +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 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..9208c73da2 --- /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:18639547.1 %
Date:2023-12-06 07:59:45Functions:111957.9 %
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 +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 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..1c63188d62 --- /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:18639547.1 %
Date:2023-12-06 07:59:45Functions:111957.9 %
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 +
78.5%78.5%
+ 		78.5 %186 / 23778.6 %11 / 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..44507960fe --- /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:2023-12-06 07:59:45Functions: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..f1df64d6b7 --- /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:2023-12-06 07:59:45Functions: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..82ce5a7ec8 --- /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:2023-12-06 07:59:45Functions: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:18623778.5 %
Date:2023-12-06 07:59:45Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getIsUtmBased()0
mrs_uav_managers::TfSource::getIsUtmSource()0
mrs_uav_managers::TfSource::setIsUtmSource(bool)0
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)42
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)42
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)42
mrs_uav_managers::TfSource::getName[abi:cxx11]()333
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> > >&)16454
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)48627
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)48627
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)52147
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)52147
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()217276
+
+
+ + + +
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..32fd5418c5 --- /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:18623778.5 %
Date:2023-12-06 07:59:45Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()217276
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)42
mrs_uav_managers::TfSource::getIsUtmBased()0
mrs_uav_managers::TfSource::getIsUtmSource()0
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_uav_managers::TfSource::setIsUtmSource(bool)0
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)42
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)48627
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> > >&)16454
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)52147
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)48627
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)52147
mrs_uav_managers::TfSource::getName[abi:cxx11]()333
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)42
+
+
+ + + +
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..d6070d6377 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html @@ -0,0 +1,701 @@ + + + + + + + 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:18623778.5 %
Date:2023-12-06 07:59:45Functions:111478.6 %
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          42 :   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          42 :       : name_(name), nh_(nh), broadcaster_(broadcaster), ch_(ch), is_utm_source_(is_utm_source) {
+      42             : 
+      43          42 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      44             : 
+      45          42 :     if (name != "dummy") {
+      46             : 
+      47             :       /*//{ load parameters */
+      48             : 
+      49          84 :       const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+      50             : 
+      51          84 :       std::string odom_topic, attitude_topic, ns;
+      52             : 
+      53          42 :       param_loader->loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
+      54          42 :       param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
+      55          42 :       if (custom_frame_id_enabled_) {
+      56           0 :         param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
+      57             :       }
+      58          42 :       param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
+      59          42 :       if (tf_from_attitude_enabled_) {
+      60          39 :         param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
+      61             :       }
+      62          42 :       param_loader->loadParam(yaml_prefix + getName() + "/namespace", ns);
+      63          42 :       full_topic_odom_     = "/" + ch_->uav_name + "/" + ns + "/" + odom_topic;
+      64          42 :       full_topic_attitude_ = "/" + ch_->uav_name + "/" + ns + "/" + attitude_topic;
+      65          42 :       param_loader->loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
+      66          42 :       param_loader->loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);
+      67             : 
+      68             :       /* coordinate frames origins //{ */
+      69          42 :       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          42 :       if (is_utm_based_) {
+      74          78 :         std::string utm_origin_parent_frame_id;
+      75          39 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
+      76          39 :         ns_utm_origin_parent_frame_id_ = ch_->uav_name + "/" + utm_origin_parent_frame_id;
+      77             : 
+      78          39 :         std::string utm_origin_child_frame_id;
+      79          39 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
+      80          39 :         ns_utm_origin_child_frame_id_ = ch_->uav_name + "/" + utm_origin_child_frame_id;
+      81             :       }
+      82             :       /*//}*/
+      83             : 
+      84             :       /*//{ world source */
+      85          42 :       if (is_utm_based_) {
+      86          78 :         std::string world_origin_parent_frame_id;
+      87          39 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
+      88          39 :         ns_world_origin_parent_frame_id_ = ch_->uav_name + "/" + world_origin_parent_frame_id;
+      89             : 
+      90          39 :         std::string world_origin_child_frame_id;
+      91          39 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
+      92          39 :         ns_world_origin_child_frame_id_ = ch_->uav_name + "/" + world_origin_child_frame_id;
+      93             :       }
+      94             :       /*//}*/
+      95             : 
+      96             :       //}
+      97             : 
+      98          42 :       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          84 :       mrs_lib::SubscribeHandlerOptions shopts;
+     107          42 :       shopts.nh                 = nh_;
+     108          42 :       shopts.node_name          = getPrintName();
+     109          42 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     110          42 :       shopts.threadsafe         = true;
+     111          42 :       shopts.autostart          = true;
+     112          42 :       shopts.queue_size         = 10;
+     113          42 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     114             : 
+     115          42 :       sh_tf_source_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, full_topic_odom_, &TfSource::callbackTfSourceOdom, this);
+     116          42 :       if (tf_from_attitude_enabled_) {
+     117          39 :         sh_tf_source_att_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this);
+     118             :       }
+     119             : 
+     120          42 :       if (is_utm_source_) {
+     121          39 :         sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     122             :       }
+     123             : 
+     124             :     }
+     125             :     /*//}*/
+     126             : 
+     127          55 :     for (auto frame_id : republish_in_frames_) {
+     128          13 :       republishers_.push_back(
+     129          26 :           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          42 :     is_initialized_ = true;
+     132          42 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     133          42 :   }
+     134             :   /*//}*/
+     135             : 
+     136             :   /*//{ getName() */
+     137         333 :   std::string getName() {
+     138         333 :     return name_;
+     139             :   }
+     140             :   /*//}*/
+     141             : 
+     142             :   /*//{ getPrintName() */
+     143      217276 :   std::string getPrintName() {
+     144      434943 :     return ch_->nodelet_name + "/" + name_;
+     145             :   }
+     146             :   /*//}*/
+     147             : 
+     148             :   /*//{ getIsUtmBased() */
+     149           0 :   bool getIsUtmBased() {
+     150           0 :     return is_utm_based_;
+     151             :   }
+     152             :   /*//}*/
+     153             : 
+     154             :   /*//{ getIsUtmSource() */
+     155           0 :   bool getIsUtmSource() {
+     156           0 :     return is_utm_source_;
+     157             :   }
+     158             :   /*//}*/
+     159             : 
+     160             :   /*//{ setIsUtmSource() */
+     161           0 :   void setIsUtmSource(const bool is_utm_source) {
+     162           0 :     if (is_utm_source) {
+     163           0 :       mrs_lib::SubscribeHandlerOptions shopts;
+     164           0 :       shopts.nh                 = nh_;
+     165           0 :       shopts.node_name          = getPrintName();
+     166           0 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     167           0 :       shopts.threadsafe         = true;
+     168           0 :       shopts.autostart          = true;
+     169           0 :       shopts.queue_size         = 10;
+     170           0 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     171           0 :       sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     172             :     }
+     173           0 :     is_utm_source_ = is_utm_source;
+     174           0 :   }
+     175             :   /*//}*/
+     176             : 
+     177             :   /*//{ setUtmOrigin() */
+     178          42 :   void setUtmOrigin(const geometry_msgs::Point& pt) {
+     179             : 
+     180          42 :     if (is_utm_based_ && !is_utm_origin_set_) {
+     181          39 :       utm_origin_        = pt;
+     182          39 :       is_utm_origin_set_ = true;
+     183             :     }
+     184          42 :   }
+     185             :   /*//}*/
+     186             : 
+     187             :   /*//{ setWorldOrigin() */
+     188          42 :   void setWorldOrigin(const geometry_msgs::Point& pt) {
+     189             : 
+     190          42 :     if (is_utm_based_ && !is_world_origin_set_) {
+     191          39 :       world_origin_        = pt;
+     192          39 :       is_world_origin_set_ = true;
+     193             :     }
+     194          42 :   }
+     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_ = true;
+     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       52147 :   void callbackTfSourceOdom(const nav_msgs::Odometry::ConstPtr msg) {
+     252             : 
+     253       52147 :     if (!is_initialized_) {
+     254           0 :       return;
+     255             :     }
+     256             : 
+     257      156441 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     258             : 
+     259       52147 :     scope_timer.checkpoint("get msg");
+     260             : 
+     261       52147 :     if (!got_first_msg_) {
+     262          42 :       first_msg_     = msg;
+     263          42 :       got_first_msg_ = true;
+     264             :     }
+     265             : 
+     266       52147 :     publishTfFromOdom(msg);
+     267       52147 :     scope_timer.checkpoint("pub tf");
+     268             : 
+     269       52147 :     if (publish_local_tf_ && !is_local_static_tf_published_) {
+     270          42 :       publishLocalTf(msg->header.frame_id);
+     271          42 :       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       68601 :     for (auto republisher : republishers_) {
+     285       16454 :       republishInFrame(msg, ch_->uav_name + "/" + republisher.first, republisher.second);
+     286             :     }
+     287             :   }
+     288             :   /*//}*/
+     289             : 
+     290             :   /*//{ callbackTfSourceAtt()*/
+     291       48627 :   void callbackTfSourceAtt(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     292             : 
+     293       48627 :     if (!is_initialized_) {
+     294           0 :       return;
+     295             :     }
+     296             : 
+     297      145881 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299       48627 :     scope_timer.checkpoint("get msg");
+     300       48627 :     publishTfFromAtt(msg);
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /* publishTfFromOdom() //{*/
+     305       52147 :   void publishTfFromOdom(const nav_msgs::OdometryConstPtr& odom) {
+     306             : 
+     307      104294 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     308             : 
+     309       52147 :     const tf2::Transform      tf       = Support::tf2FromPose(odom->pose.pose);
+     310       52147 :     const tf2::Transform      tf_inv   = tf.inverse();
+     311       52147 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     312             : 
+     313             :     /*//{ tf source origin */
+     314       52147 :     geometry_msgs::TransformStamped tf_msg;
+     315       52147 :     tf_msg.header.stamp         = odom->header.stamp;
+     316       52147 :     std::string origin_frame_id = custom_frame_id_enabled_ ? ch_->uav_name + "/" + custom_frame_id_ : odom->header.frame_id;
+     317       52147 :     if (is_inverted_) {
+     318             : 
+     319       52147 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     320       52147 :       tf_msg.child_frame_id        = origin_frame_id;
+     321       52147 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     322       52147 :       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       52147 :     if (Support::noNans(tf_msg)) {
+     332             :       try {
+     333       52147 :         broadcaster_->sendTransform(tf_msg);
+     334             :       }
+     335           0 :       catch (...) {
+     336           0 :         ROS_ERROR("exception caught ");
+     337             :       }
+     338             : 
+     339       52147 :       if (tf_from_attitude_enabled_) {
+     340       48007 :         if (is_inverted_) {
+     341       48007 :           tf_msg.child_frame_id += "_att_only";
+     342             :         } else {
+     343           0 :           tf_msg.header.frame_id += "_att_only";
+     344             :         }
+     345             :         try {
+     346       48007 :           broadcaster_->sendTransform(tf_msg);
+     347             :         }
+     348           0 :         catch (...) {
+     349           0 :           ROS_ERROR("exception caught ");
+     350             :         }
+     351             :       }
+     352             :       /*//}*/
+     353             : 
+     354             :       /*//{ tf utm origin */
+     355             : 
+     356       52147 :       geometry_msgs::TransformStamped tf_utm_msg;
+     357       52147 :       if (is_utm_source_) {
+     358             :         
+     359       48007 :         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       48007 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     365       48007 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     366       48007 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     367             : 
+     368       48007 :         if (sh_altitude_amsl_.hasMsg()) {
+     369       48007 :           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       48007 :         tf_utm_msg.header.stamp    = odom->header.stamp;
+     375       48007 :         tf_utm_msg.header.frame_id = ns_utm_origin_parent_frame_id_;
+     376       48007 :         tf_utm_msg.child_frame_id  = ns_utm_origin_child_frame_id_;
+     377             : 
+     378       48007 :         tf2::Transform tf_utm;
+     379       48007 :         if (is_inverted_) {
+     380       48007 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     381             :         } else {
+     382           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     383             :         }
+     384       48007 :         tf_utm_msg.transform = Support::msgFromTf2(tf_utm);
+     385             : 
+     386             :         try {
+     387       48007 :           broadcaster_->sendTransform(tf_utm_msg);
+     388       48007 :           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       52147 :       if (is_utm_source_) {
+     398       96014 :         geometry_msgs::TransformStamped tf_world_msg;
+     399       48007 :         tf_world_msg.header.stamp    = odom->header.stamp;
+     400       48007 :         tf_world_msg.header.frame_id = ns_world_origin_parent_frame_id_;
+     401       48007 :         tf_world_msg.child_frame_id  = ns_world_origin_child_frame_id_;
+     402             : 
+     403       48007 :         tf2::Transform tf_world;
+     404       48007 :         tf_world.setOrigin(tf2::Vector3(world_origin_.x, world_origin_.y, world_origin_.z));
+     405       48007 :         tf_world.setRotation(tf2::Quaternion(0, 0, 0, 1));
+     406             : 
+     407       48007 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     408       48007 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     409       48007 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     410             : 
+     411       48007 :         tf2::Transform tf_utm;
+     412       48007 :         if (is_inverted_) {
+     413       48007 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     414             :         } else {
+     415           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     416             :         }
+     417             : 
+     418       48007 :         tf_world_msg.transform          = Support::msgFromTf2(tf_utm * tf_world);
+     419       48007 :         tf_world_msg.transform.rotation = pose_inv.orientation;
+     420             : 
+     421             :         try {
+     422       48007 :           broadcaster_->sendTransform(tf_world_msg);
+     423       48007 :           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       52147 :     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       48627 :   void publishTfFromAtt(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     443             : 
+     444      145881 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     445             : 
+     446       97254 :     geometry_msgs::TransformStamped tf_msg;
+     447       48627 :     tf_msg.header.stamp = msg->header.stamp;
+     448             : 
+     449       48627 :     geometry_msgs::Pose pose;
+     450       48627 :     pose.orientation = msg->quaternion;
+     451       48627 :     if (is_inverted_) {
+     452             : 
+     453       48627 :       const tf2::Transform      tf       = Support::tf2FromPose(pose);
+     454       48627 :       const tf2::Transform      tf_inv   = tf.inverse();
+     455       48627 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     456             : 
+     457       48627 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     458       48627 :       tf_msg.child_frame_id        = msg->header.frame_id;
+     459       48627 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     460       48627 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     461             : 
+     462             :     } else {
+     463           0 :       tf_msg.header.frame_id       = msg->header.frame_id;
+     464           0 :       tf_msg.child_frame_id        = ch_->frames.ns_fcu;
+     465           0 :       tf_msg.transform.translation = Support::pointToVector3(pose.position);
+     466           0 :       tf_msg.transform.rotation    = pose.orientation;
+     467             :     }
+     468             : 
+     469       48627 :     if (Support::noNans(tf_msg)) {
+     470             :       try {
+     471       48627 :         scope_timer.checkpoint("before pub");
+     472       48627 :         broadcaster_->sendTransform(tf_msg);
+     473       48627 :         scope_timer.checkpoint("after pub");
+     474             :       }
+     475           0 :       catch (...) {
+     476           0 :         ROS_ERROR("exception caught ");
+     477             :       }
+     478             :     } else {
+     479           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(),
+     480             :                         tf_msg.child_frame_id.c_str());
+     481             :     }
+     482       48627 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     483             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_attitude_.c_str());
+     484       48627 :   }
+     485             :   /*//}*/
+     486             : 
+     487             :   /* publishLocalTf() //{*/
+     488          42 :   void publishLocalTf(const std::string& frame_id) {
+     489             : 
+     490         126 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     491             : 
+     492          84 :     geometry_msgs::TransformStamped tf_msg;
+     493          42 :     tf_msg.header.stamp = ros::Time::now();
+     494             : 
+     495          42 :     tf_msg.header.frame_id       = frame_id;
+     496          42 :     tf_msg.child_frame_id        = frame_id.substr(0, frame_id.find("_origin")) + "_local_origin";
+     497          42 :     tf_msg.transform.translation = Support::pointToVector3(first_msg_->pose.pose.position);
+     498          42 :     tf_msg.transform.rotation    = first_msg_->pose.pose.orientation;
+     499             : 
+     500          42 :     if (Support::noNans(tf_msg)) {
+     501             :       try {
+     502          42 :         static_broadcaster_.sendTransform(tf_msg);
+     503             :       }
+     504           0 :       catch (...) {
+     505           0 :         ROS_ERROR("exception caught ");
+     506             :       }
+     507             :     } else {
+     508           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(),
+     509             :                         tf_msg.child_frame_id.c_str());
+     510             :     }
+     511          42 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     512             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     513          42 :     is_local_static_tf_published_ = true;
+     514          42 :   }
+     515             :   /*//}*/
+     516             : 
+     517             :   /* publishUtmTf() //{*/
+     518             :   void publishUtmTf(const std::string& frame_id) {
+     519             : 
+     520             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishUtmTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     521             : 
+     522             :     geometry_msgs::TransformStamped tf_msg;
+     523             :     tf_msg.header.stamp = ros::Time::now();
+     524             : 
+     525             :     tf_msg.header.frame_id         = frame_id;
+     526             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_utm_origin";
+     527             :     tf_msg.transform.translation.x = -utm_origin_.x;  // minus because inverse tf tree
+     528             :     tf_msg.transform.translation.y = -utm_origin_.y;  // minus because inverse tf tree
+     529             :     tf_msg.transform.translation.z = -utm_origin_.z;  // minus because inverse tf tree
+     530             :     tf_msg.transform.rotation.x    = 0;
+     531             :     tf_msg.transform.rotation.y    = 0;
+     532             :     tf_msg.transform.rotation.z    = 0;
+     533             :     tf_msg.transform.rotation.w    = 1;
+     534             : 
+     535             :     if (Support::noNans(tf_msg)) {
+     536             :       try {
+     537             :         static_broadcaster_.sendTransform(tf_msg);
+     538             :       }
+     539             :       catch (...) {
+     540             :         ROS_ERROR("exception caught ");
+     541             :       }
+     542             :     } else {
+     543             :       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(),
+     544             :                         tf_msg.child_frame_id.c_str());
+     545             :     }
+     546             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     547             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     548             :     is_utm_static_tf_published_ = true;
+     549             :   }
+     550             :   /*//}*/
+     551             : 
+     552             :   /* publishWorldTf() //{*/
+     553             :   void publishWorldTf(const std::string& frame_id) {
+     554             : 
+     555             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishWorldTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     556             : 
+     557             :     geometry_msgs::TransformStamped tf_msg;
+     558             :     tf_msg.header.stamp = ros::Time::now();
+     559             : 
+     560             :     tf_msg.header.frame_id         = frame_id;
+     561             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_world_origin";
+     562             :     tf_msg.transform.translation.x = -(utm_origin_.x - world_origin_.x);  // minus because inverse tf tree
+     563             :     tf_msg.transform.translation.y = -(utm_origin_.y - world_origin_.y);  // minus because inverse tf tree
+     564             :     /* tf_msg.transform.translation.z = -(utm_origin_.z);                    // minus because inverse tf tree */
+     565             :     tf_msg.transform.translation.z = 0;
+     566             :     tf_msg.transform.rotation.x    = 0;
+     567             :     tf_msg.transform.rotation.y    = 0;
+     568             :     tf_msg.transform.rotation.z    = 0;
+     569             :     tf_msg.transform.rotation.w    = 1;
+     570             : 
+     571             :     if (Support::noNans(tf_msg)) {
+     572             :       try {
+     573             :         static_broadcaster_.sendTransform(tf_msg);
+     574             :       }
+     575             :       catch (...) {
+     576             :         ROS_ERROR("exception caught ");
+     577             :       }
+     578             :     } else {
+     579             :       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(),
+     580             :                         tf_msg.child_frame_id.c_str());
+     581             :     }
+     582             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     583             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     584             :     is_world_static_tf_published_ = true;
+     585             :   }
+     586             :   /*//}*/
+     587             : 
+     588             :   /* republishInFrame() //{*/
+     589       16454 :   void republishInFrame(const nav_msgs::OdometryConstPtr& msg, const std::string& frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>& ph) {
+     590             : 
+     591       32908 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::republishInFrame", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     592             : 
+     593       16454 :     nav_msgs::Odometry msg_out = *msg;
+     594       16454 :     msg_out.header.frame_id    = frame_id;
+     595             : 
+     596       16454 :     geometry_msgs::PoseStamped pose;
+     597       16454 :     pose.header = msg->header;
+     598       16454 :     pose.pose   = msg->pose.pose;
+     599             : 
+     600       16454 :     auto res = ch_->transformer->transformSingle(pose, frame_id);
+     601       16454 :     if (res) {
+     602       13931 :       msg_out.pose.pose = res->pose;
+     603       13931 :       ph.publish(msg_out);
+     604             :     } else {
+     605        2523 :       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());
+     606        2523 :       return;
+     607             :     }
+     608             :   }
+     609             : 
+     610             :   /*//}*/
+     611             : };
+     612             : 
+     613             : /*//}*/
+     614             : 
+     615             : }  // namespace mrs_uav_managers
+     616             : 
+     617             : #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..61645dbed1 --- /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..fffaa91ed328119a62fbcaa8a472c597b693e295 GIT binary patch literal 2207 zcmV;Q2w?Y#P);hgBY8c=wazLAiaeYF^1WlAS{M;cfg(+1JJEGR@nc<|aydlt3@Jph$(9<} z-8I+Lz%eyALufvJowT{@CGt^f$JSU!d+$_FHQe$9Y?)b~D0KoBB0g60k@7Z`1 zaK>BDn3;Me*dANAjlG?;H6NPmdh{e%AZP5;wkF2b@R()=3}lV{Zd+eVZ)0N*^^anT zd)R-EpX299@5AfyQvj<|oO&+oQqOA!SK6&eBaCx!#vRjKm^{cZAP7uOot8)yAogt$ zXaYsnk-4s$z(7%0AeB63VF23+sA>hpUD`0*Mi)rz@)l532LN4zJPK>fxAWMsalU#G zK4p%(Mrdjpq_%P>)b&_E1rM5(YJ~ejX|xbv@gL!MQue&L{j~<$q45y(ntMku2iN>I zNWHiD)Jy%sP~TiN7lxD9Qc3Cn^eY`kzz>nSZti=W=%4>LCib)@_^HkClWzXED}Yx@ zT{kw6`lt&7q^>P|zD;Vos7uzwtXrwIPC+sx4^B}Ln0h({Z9<9>$hsWv8fIif;1MXY zw&J>O0s}?f_=v|W3}Bm5q@dg;(uCqsd%<3KYE)yw%BaYh)%_-i3nbqLwsW(0A ziGXAboB8m6+FX*WMzLJFR)E^Y$NQHGP18Wj)J${MGSoC#;SJM}{G){Y3d`uvf$~94 z;}a0*AN1_VOs?8ufF6pt&E%YjK)f(BdVjv_6Y_C#LipPbCnA$tj&ERdti zQsjY!&6Wy9F9JSE)&ftsZ|)?~T-foGdz;Vp2`QojUzRQpJlA!60>gw88%uprq|gK& zEOn*IrsPzzU=NRk;z?mTx-QfmrJy4R1LIpI@C?OnVGz|ZVuT{cY<~~z=aayIA`gss z4X{d)XKV?N3JK;b%TWOFrOIV&8_K<*q~;!%b0f>bMl#iJrP>=6{Z>ldz&cZ`r?BhK zfx47TRRRv^v2|YPKyGc_$X+Y9Jw40o5rr$$vq_Kgk%o@P!XHVZd51uHQpslv874X8 z3-oAGLgpfz5U@1#ti{JUn5Ya$eh2X5A+nzg_zo^R-&~yH)$@+5c>;Lz9qm^Mp2dk zJA1hLoLe#bu}MOJxAj}GbkJ=@?k$Do^xGX z%e8ft?I;E;uK9>dalVAr)b~6|kA4(hZ?kLm-lbx}-mNEp#8yf7_EqA(h-z*37| z(IxepSCF%xldd2~?P6Fz>nr-wv|=b!V-GT+x1kG>2l$l_q^iYhpNDJph?YqFWQc)z zCP%FgIHYjNl4!WnrJjMnvDlO9G|IBvS6c?HDP+sAx)y*20}a%p+A^WjTdNYVYx>|< zS_s%>Lb8e(9$Y|b%IVPMG~3QhNJRA*=y){Y5#K}low*tmmylnoOH>qy*uGcG~LRP=1y7QkT7y606-y?34pK2 zy4Dlo0#HbZm%vIw{I6z)Z)EXNGW1oM;d0KtIy21N1ppNBJoA;AVfKDeR|*+lco%_R zkZb-QXVNV{jc= z06PLTa3G5N{woQm?OGX{i=IQ*?kRkUf_<kImcL}_lshmvB=6fu-q>-RQY!~ z`h7wgrxlywMURVOGotq!nk(J|0%`ul_?LYuKPML%G#o6#BZD%<8kMc4m<22 zBnz4+zD1fl2-F5Pt|6$-nn{M;+=2`G{FOU&_iqBQy(yhuaM4CT3YFvfx^E!p?OETJ h6s*MwtFvi|`~fwtzAyX38ma&Q002ovPDHLkV1kIQEMouw 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..38900a0cd3 --- /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:14822765.2 %
Date:2023-12-06 07:59:45Functions: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()42
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)42
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()42
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)676
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&)1134
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)6903
+
+
+ + + +
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..350c7733d7 --- /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:14822765.2 %
Date:2023-12-06 07:59:45Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)42
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&)1134
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)676
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&)6903
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()42
+
+
+ + + +
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..2dfbd41ecd --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.html @@ -0,0 +1,706 @@ + + + + + + + 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:14822765.2 %
Date:2023-12-06 07:59:45Functions: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             :   bool            is_initialized_ = false;
+      45             : 
+      46             :   // | ----------------------- parameters ----------------------- |
+      47             : 
+      48             :   std::vector<std::string> _estimator_type_names_;
+      49             : 
+      50             :   std::vector<std::string>                                       _constraint_names_;
+      51             :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest> _constraints_;
+      52             : 
+      53             :   std::map<std::string, std::vector<std::string>> _map_type_allowed_constraints_;
+      54             :   std::map<std::string, std::string>              _map_type_default_constraints_;
+      55             : 
+      56             :   // | --------------------- service clients -------------------- |
+      57             : 
+      58             :   mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> sc_set_constraints_;
+      59             : 
+      60             :   // | ----------------------- subscribers ---------------------- |
+      61             : 
+      62             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+      63             : 
+      64             :   // | ------------- constraint management ------------- |
+      65             : 
+      66             :   bool setConstraints(std::string constraints_names);
+      67             : 
+      68             :   ros::ServiceServer service_server_set_constraints_;
+      69             :   bool               callbackSetConstraints(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+      70             : 
+      71             :   std::string last_estimator_name_;
+      72             :   std::mutex  mutex_last_estimator_name_;
+      73             : 
+      74             :   void       timerConstraintManagement(const ros::TimerEvent& event);
+      75             :   ros::Timer timer_constraint_management_;
+      76             :   double     _constraint_management_rate_;
+      77             : 
+      78             :   std::string current_constraints_;
+      79             :   std::mutex  mutex_current_constraints_;
+      80             : 
+      81             :   // | ------------------ constraints override ------------------ |
+      82             : 
+      83             :   ros::ServiceServer service_server_constraints_override_;
+      84             :   bool               callbackConstraintsOverride(mrs_msgs::ConstraintsOverride::Request& req, mrs_msgs::ConstraintsOverride::Response& res);
+      85             : 
+      86             :   std::atomic<bool>                    override_constraints_         = false;
+      87             :   std::atomic<bool>                    constraints_override_updated_ = false;
+      88             :   std::mutex                           mutex_constraints_override_;
+      89             :   mrs_msgs::ConstraintsOverrideRequest constraints_override_;
+      90             : 
+      91             :   // | ------------------ diagnostics publisher ----------------- |
+      92             : 
+      93             :   mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics> ph_diagnostics_;
+      94             : 
+      95             :   void       timerDiagnostics(const ros::TimerEvent& event);
+      96             :   ros::Timer timer_diagnostics_;
+      97             :   double     _diagnostics_rate_;
+      98             : 
+      99             :   // | ------------------------ profiler ------------------------ |
+     100             : 
+     101             :   mrs_lib::Profiler profiler_;
+     102             :   bool              _profiler_enabled_ = false;
+     103             : 
+     104             :   // | ------------------- scope timer logger ------------------- |
+     105             : 
+     106             :   bool                                       scope_timer_enabled_ = false;
+     107             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     108             : 
+     109             :   // | ------------------------- helpers ------------------------ |
+     110             : 
+     111             :   bool stringInVector(const std::string& value, const std::vector<std::string>& vector);
+     112             : };
+     113             : 
+     114             : //}
+     115             : 
+     116             : /* //{ onInit() */
+     117             : 
+     118          42 : void ConstraintManager::onInit() {
+     119             : 
+     120          42 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     121             : 
+     122          42 :   ros::Time::waitForValid();
+     123             : 
+     124          42 :   ROS_INFO("[ConstraintManager]: initializing");
+     125             : 
+     126             :   // | ------------------------- params ------------------------- |
+     127             : 
+     128          84 :   mrs_lib::ParamLoader param_loader(nh_, "ConstraintManager");
+     129             : 
+     130          84 :   std::string custom_config_path;
+     131          84 :   std::string platform_config_path;
+     132             : 
+     133          42 :   param_loader.loadParam("custom_config", custom_config_path);
+     134          42 :   param_loader.loadParam("platform_config", platform_config_path);
+     135             : 
+     136          42 :   if (custom_config_path != "") {
+     137          42 :     param_loader.addYamlFile(custom_config_path);
+     138             :   }
+     139             : 
+     140          42 :   if (platform_config_path != "") {
+     141          42 :     param_loader.addYamlFile(platform_config_path);
+     142             :   }
+     143             : 
+     144          42 :   param_loader.addYamlFileFromParam("private_config");
+     145          42 :   param_loader.addYamlFileFromParam("public_config");
+     146          42 :   param_loader.addYamlFileFromParam("public_constraints");
+     147             : 
+     148          84 :   const std::string yaml_prefix = "mrs_uav_managers/constraint_manager/";
+     149             : 
+     150             :   // params passed from the launch file are not prefixed
+     151          42 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     152             : 
+     153          42 :   param_loader.loadParam(yaml_prefix + "constraints", _constraint_names_);
+     154             : 
+     155          42 :   param_loader.loadParam(yaml_prefix + "estimator_types", _estimator_type_names_);
+     156             : 
+     157          42 :   param_loader.loadParam(yaml_prefix + "rate", _constraint_management_rate_);
+     158          42 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     159             : 
+     160          42 :   std::vector<std::string>::iterator it;
+     161             : 
+     162             :   // loading constraint names
+     163         168 :   for (it = _constraint_names_.begin(); it != _constraint_names_.end(); ++it) {
+     164             : 
+     165         126 :     ROS_INFO_STREAM("[ConstraintManager]: loading constraints '" << *it << "'");
+     166             : 
+     167         126 :     mrs_msgs::DynamicsConstraintsSrvRequest new_constraints;
+     168             : 
+     169         126 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/speed", new_constraints.constraints.horizontal_speed);
+     170         126 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/acceleration", new_constraints.constraints.horizontal_acceleration);
+     171         126 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/jerk", new_constraints.constraints.horizontal_jerk);
+     172         126 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/snap", new_constraints.constraints.horizontal_snap);
+     173             : 
+     174         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/speed", new_constraints.constraints.vertical_ascending_speed);
+     175         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/acceleration", new_constraints.constraints.vertical_ascending_acceleration);
+     176         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/jerk", new_constraints.constraints.vertical_ascending_jerk);
+     177         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/snap", new_constraints.constraints.vertical_ascending_snap);
+     178             : 
+     179         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/speed", new_constraints.constraints.vertical_descending_speed);
+     180         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/acceleration", new_constraints.constraints.vertical_descending_acceleration);
+     181         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/jerk", new_constraints.constraints.vertical_descending_jerk);
+     182         126 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/snap", new_constraints.constraints.vertical_descending_snap);
+     183             : 
+     184         126 :     param_loader.loadParam(yaml_prefix + *it + "/heading/speed", new_constraints.constraints.heading_speed);
+     185         126 :     param_loader.loadParam(yaml_prefix + *it + "/heading/acceleration", new_constraints.constraints.heading_acceleration);
+     186         126 :     param_loader.loadParam(yaml_prefix + *it + "/heading/jerk", new_constraints.constraints.heading_jerk);
+     187         126 :     param_loader.loadParam(yaml_prefix + *it + "/heading/snap", new_constraints.constraints.heading_snap);
+     188             : 
+     189         126 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/roll", new_constraints.constraints.roll_rate);
+     190         126 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/pitch", new_constraints.constraints.pitch_rate);
+     191         126 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/yaw", new_constraints.constraints.yaw_rate);
+     192             : 
+     193             :     double tilt_deg;
+     194             : 
+     195         126 :     param_loader.loadParam(yaml_prefix + *it + "/tilt", tilt_deg);
+     196             : 
+     197         126 :     new_constraints.constraints.tilt = M_PI * (tilt_deg / 180.0);
+     198             : 
+     199         126 :     _constraints_.insert(std::pair<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>(*it, new_constraints));
+     200             :   }
+     201             : 
+     202             :   // loading the allowed constraints lists
+     203         336 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     204             : 
+     205         294 :     std::vector<std::string> temp_vector;
+     206         294 :     param_loader.loadParam(yaml_prefix + "allowed_constraints/" + *it, temp_vector);
+     207             : 
+     208         294 :     std::vector<std::string>::iterator it2;
+     209        1092 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     210         798 :       if (!stringInVector(*it2, _constraint_names_)) {
+     211           0 :         ROS_ERROR("[ConstraintManager]: the element '%s' of %s/allowed_constraints is not a valid constraint!", it2->c_str(), it->c_str());
+     212           0 :         ros::shutdown();
+     213             :       }
+     214             :     }
+     215             : 
+     216         294 :     _map_type_allowed_constraints_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     217             :   }
+     218             : 
+     219             :   // loading the default constraints
+     220         336 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     221             : 
+     222         294 :     std::string temp_str;
+     223         294 :     param_loader.loadParam(yaml_prefix + "default_constraints/" + *it, temp_str);
+     224             : 
+     225         294 :     if (!stringInVector(temp_str, _map_type_allowed_constraints_.at(*it))) {
+     226           0 :       ROS_ERROR("[ConstraintManager]: the element '%s' of %s/allowed_constraints is not a valid constraint!", temp_str.c_str(), it->c_str());
+     227           0 :       ros::shutdown();
+     228             :     }
+     229             : 
+     230         294 :     _map_type_default_constraints_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     231             :   }
+     232             : 
+     233          42 :   ROS_INFO("[ConstraintManager]: done loading dynamical params");
+     234             : 
+     235          42 :   current_constraints_ = "";
+     236          42 :   last_estimator_name_ = "";
+     237             : 
+     238             :   // | ------------------------ services ------------------------ |
+     239             : 
+     240          42 :   service_server_set_constraints_ = nh_.advertiseService("set_constraints_in", &ConstraintManager::callbackSetConstraints, this);
+     241             : 
+     242          42 :   service_server_constraints_override_ = nh_.advertiseService("constraints_override_in", &ConstraintManager::callbackConstraintsOverride, this);
+     243             : 
+     244          42 :   sc_set_constraints_ = mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>(nh_, "set_constraints_out");
+     245             : 
+     246             :   // | ----------------------- subscribers ---------------------- |
+     247             : 
+     248          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+     249          42 :   shopts.nh                 = nh_;
+     250          42 :   shopts.node_name          = "ConstraintManager";
+     251          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     252          42 :   shopts.threadsafe         = true;
+     253          42 :   shopts.autostart          = true;
+     254          42 :   shopts.queue_size         = 10;
+     255          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     256             : 
+     257          42 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     258             : 
+     259             :   // | ----------------------- publishers ----------------------- |
+     260             : 
+     261          42 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     262             : 
+     263             :   // | ------------------------- timers ------------------------- |
+     264             : 
+     265          42 :   timer_constraint_management_ = nh_.createTimer(ros::Rate(_constraint_management_rate_), &ConstraintManager::timerConstraintManagement, this);
+     266          42 :   timer_diagnostics_           = nh_.createTimer(ros::Rate(_diagnostics_rate_), &ConstraintManager::timerDiagnostics, this);
+     267             : 
+     268             :   // | ------------------------ profiler ------------------------ |
+     269             : 
+     270          42 :   profiler_ = mrs_lib::Profiler(nh_, "ConstraintManager", _profiler_enabled_);
+     271             : 
+     272             :   // | ------------------- scope timer logger ------------------- |
+     273             : 
+     274          42 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     275         126 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     276          42 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     277             : 
+     278             :   // | ----------------------- finish init ---------------------- |
+     279             : 
+     280          42 :   if (!param_loader.loadedSuccessfully()) {
+     281           0 :     ROS_ERROR("[ConstraintManager]: Could not load all parameters!");
+     282           0 :     ros::shutdown();
+     283             :   }
+     284             : 
+     285          42 :   is_initialized_ = true;
+     286             : 
+     287          42 :   ROS_INFO("[ConstraintManager]: initialized");
+     288             : 
+     289          42 :   ROS_DEBUG("[ConstraintManager]: debug output is enabled");
+     290          42 : }
+     291             : 
+     292             : //}
+     293             : 
+     294             : // --------------------------------------------------------------
+     295             : // |                           methods                          |
+     296             : // --------------------------------------------------------------
+     297             : 
+     298             : /* setConstraints() //{ */
+     299             : 
+     300          42 : bool ConstraintManager::setConstraints(std::string constraints_name) {
+     301             : 
+     302          42 :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     303          42 :   it = _constraints_.find(constraints_name);
+     304             : 
+     305          42 :   if (it == _constraints_.end()) {
+     306           0 :     ROS_ERROR("[ConstraintManager]: could not setConstraints(), the constraint name '%s' is not on the list", constraints_name.c_str());
+     307           0 :     return false;
+     308             :   }
+     309             : 
+     310          84 :   mrs_msgs::DynamicsConstraintsSrv srv_call;
+     311             : 
+     312          42 :   srv_call.request = it->second;
+     313             : 
+     314          42 :   if (override_constraints_) {
+     315             : 
+     316           0 :     auto constraints_override = mrs_lib::get_mutexed(mutex_constraints_override_, constraints_override_);
+     317             : 
+     318           0 :     if (constraints_override.acceleration_horizontal > 0 &&
+     319           0 :         constraints_override.acceleration_horizontal <= srv_call.request.constraints.horizontal_acceleration) {
+     320           0 :       srv_call.request.constraints.horizontal_acceleration = constraints_override.acceleration_horizontal;
+     321             :     } else {
+     322           0 :       ROS_ERROR_THROTTLE(1.0, "[ConstraintManager]: required horizontal acceleration override is out of bounds");
+     323             :     }
+     324             : 
+     325           0 :     if (constraints_override.acceleration_vertical > 0 &&
+     326           0 :         constraints_override.acceleration_vertical <= srv_call.request.constraints.vertical_ascending_acceleration &&
+     327           0 :         constraints_override.acceleration_vertical <= srv_call.request.constraints.vertical_descending_acceleration) {
+     328           0 :       srv_call.request.constraints.vertical_ascending_acceleration  = constraints_override.acceleration_vertical;
+     329           0 :       srv_call.request.constraints.vertical_descending_acceleration = constraints_override.acceleration_vertical;
+     330             :     } else {
+     331           0 :       ROS_ERROR_THROTTLE(1.0, "[ConstraintManager]: required vertical acceleration override is out of bounds");
+     332             :     }
+     333             :   }
+     334             : 
+     335          42 :   bool res = sc_set_constraints_.call(srv_call);
+     336             : 
+     337          42 :   if (!res) {
+     338             : 
+     339           0 :     ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the service for setting constraints has failed!");
+     340           0 :     return false;
+     341             : 
+     342             :   } else {
+     343             : 
+     344          42 :     if (srv_call.response.success) {
+     345             : 
+     346          42 :       mrs_lib::set_mutexed(mutex_current_constraints_, constraints_name, current_constraints_);
+     347          42 :       return true;
+     348             : 
+     349             :     } else {
+     350             : 
+     351           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: set service for setting constraints returned: '%s'", srv_call.response.message.c_str());
+     352           0 :       return false;
+     353             :     }
+     354             :   }
+     355             : }
+     356             : 
+     357             : //}
+     358             : 
+     359             : // --------------------------------------------------------------
+     360             : // |                          callbacks                         |
+     361             : // --------------------------------------------------------------
+     362             : 
+     363             : // | -------------------- service callbacks ------------------- |
+     364             : 
+     365             : /* //{ callbackSetConstraints() */
+     366             : 
+     367           0 : bool ConstraintManager::callbackSetConstraints(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+     368             : 
+     369           0 :   if (!is_initialized_) {
+     370           0 :     return false;
+     371             :   }
+     372             : 
+     373           0 :   std::stringstream ss;
+     374             : 
+     375           0 :   if (!sh_estimation_diag_.hasMsg()) {
+     376             : 
+     377           0 :     ss << "missing odometry diagnostics";
+     378             : 
+     379           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     380             : 
+     381           0 :     res.message = ss.str();
+     382           0 :     res.success = false;
+     383           0 :     return true;
+     384             :   }
+     385             : 
+     386           0 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     387             : 
+     388           0 :   if (!stringInVector(req.value, _constraint_names_)) {
+     389             : 
+     390           0 :     ss << "the constraints '" << req.value.c_str() << "' do not exist (in the ConstraintManager's config)";
+     391             : 
+     392           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     393             : 
+     394           0 :     res.message = ss.str();
+     395           0 :     res.success = false;
+     396           0 :     return true;
+     397             :   }
+     398             : 
+     399           0 :   if (!stringInVector(req.value, _map_type_allowed_constraints_.at(estimation_diagnostics.current_state_estimator))) {
+     400             : 
+     401           0 :     ss << "the constraints '" << req.value.c_str() << "' are not allowed given the current odometry type";
+     402             : 
+     403           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     404             : 
+     405           0 :     res.message = ss.str();
+     406           0 :     res.success = false;
+     407           0 :     return true;
+     408             :   }
+     409             : 
+     410           0 :   override_constraints_ = false;
+     411             : 
+     412             :   // try to set the constraints
+     413           0 :   if (!setConstraints(req.value)) {
+     414             : 
+     415           0 :     ss << "the ControlManager could not set the constraints";
+     416             : 
+     417           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     418             : 
+     419           0 :     res.message = ss.str();
+     420           0 :     res.success = false;
+     421           0 :     return true;
+     422             : 
+     423             :   } else {
+     424             : 
+     425           0 :     ss << "the constraints '" << req.value.c_str() << "' were set";
+     426             : 
+     427           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     428             : 
+     429           0 :     res.message = ss.str();
+     430           0 :     res.success = true;
+     431           0 :     return true;
+     432             :   }
+     433             : }
+     434             : 
+     435             : //}
+     436             : 
+     437             : /* callackConstraintsOverride() //{ */
+     438             : 
+     439           0 : bool ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverride::Request& req, mrs_msgs::ConstraintsOverride::Response& res) {
+     440             : 
+     441           0 :   if (!is_initialized_) {
+     442           0 :     return false;
+     443             :   }
+     444             : 
+     445             :   {
+     446           0 :     std::scoped_lock lock(mutex_constraints_override_);
+     447             : 
+     448           0 :     constraints_override_ = req;
+     449             :   }
+     450             : 
+     451           0 :   override_constraints_         = true;
+     452           0 :   constraints_override_updated_ = true;
+     453             : 
+     454           0 :   ROS_INFO_THROTTLE(0.1, "[ConstraintManager]: setting constraints override");
+     455             : 
+     456           0 :   res.message = "override set";
+     457           0 :   res.success = true;
+     458             : 
+     459           0 :   return true;
+     460             : }
+     461             : 
+     462             : //}
+     463             : 
+     464             : // --------------------------------------------------------------
+     465             : // |                           timers                           |
+     466             : // --------------------------------------------------------------
+     467             : 
+     468             : /* timerConstraintManagement() //{ */
+     469             : 
+     470        6903 : void ConstraintManager::timerConstraintManagement(const ros::TimerEvent& event) {
+     471             : 
+     472        6903 :   if (!is_initialized_) {
+     473        2122 :     return;
+     474             :   }
+     475             : 
+     476       13806 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerConstraintManagement", _constraint_management_rate_, 0.01, event);
+     477       13806 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerConstraintManagement", scope_timer_logger_, scope_timer_enabled_);
+     478             : 
+     479        6903 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     480        6903 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     481             : 
+     482        6903 :   if (!sh_estimation_diag_.hasMsg()) {
+     483        2122 :     return;
+     484             :   }
+     485             : 
+     486        4781 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     487             : 
+     488             :   // | --- automatically set constraints when the state estimator changes -- |
+     489        4781 :   if (estimation_diagnostics.current_state_estimator != last_estimator_name_) {
+     490             : 
+     491          42 :     ROS_INFO_THROTTLE(1.0, "[ConstraintManager]: the state estimator has changed! %s -> %s", last_estimator_name_.c_str(),
+     492             :                       estimation_diagnostics.current_state_estimator.c_str());
+     493             : 
+     494          42 :     std::map<std::string, std::string>::iterator it;
+     495          42 :     it = _map_type_default_constraints_.find(estimation_diagnostics.current_state_estimator);
+     496             : 
+     497          42 :     if (it == _map_type_default_constraints_.end()) {
+     498             : 
+     499           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the state estimator type '%s' was not specified in the constraint_manager's config!",
+     500             :                         estimation_diagnostics.current_state_estimator.c_str());
+     501             : 
+     502             :     } else {
+     503             : 
+     504             :       // if the current constraints are within the allowed state estimator types, do nothing
+     505          42 :       if (stringInVector(current_constraints, _map_type_allowed_constraints_.at(estimation_diagnostics.current_state_estimator))) {
+     506             : 
+     507           0 :         last_estimator_name = estimation_diagnostics.current_state_estimator;
+     508             : 
+     509             :         // else, try to set the initial constraints
+     510             :       } else {
+     511             : 
+     512          42 :         ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the current constraints '%s' are not within the allowed constraints for '%s'", current_constraints.c_str(),
+     513             :                           estimation_diagnostics.current_state_estimator.c_str());
+     514             : 
+     515          42 :         if (setConstraints(it->second)) {
+     516             : 
+     517          42 :           last_estimator_name = estimation_diagnostics.current_state_estimator;
+     518             : 
+     519          42 :           ROS_INFO_THROTTLE(1.0, "[ConstraintManager]: constraints set to initial: '%s'", it->second.c_str());
+     520             : 
+     521             :         } else {
+     522             : 
+     523           0 :           ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: could not set constraints!");
+     524             :         }
+     525             :       }
+     526             :     }
+     527             :   }
+     528             : 
+     529        4781 :   if (constraints_override_updated_) {
+     530             : 
+     531           0 :     std::map<std::string, std::string>::iterator it;
+     532           0 :     it = _map_type_default_constraints_.find(last_estimator_name_);
+     533             : 
+     534           0 :     ROS_INFO_THROTTLE(0.1, "[ConstraintManager]: re-setting constraints with user value override");
+     535             : 
+     536           0 :     if (setConstraints(it->second)) {
+     537           0 :       constraints_override_updated_ = false;
+     538             :     } else {
+     539           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: could not re-set the constraints!");
+     540             :     }
+     541             :   }
+     542             : 
+     543        4781 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     544             : }
+     545             : 
+     546             : //}
+     547             : 
+     548             : /* timerDiagnostics() //{ */
+     549             : 
+     550         676 : void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
+     551             : 
+     552         676 :   if (!is_initialized_) {
+     553         208 :     return;
+     554             :   }
+     555             : 
+     556        1352 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     557        1352 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     558             : 
+     559         676 :   if (!sh_estimation_diag_.hasMsg()) {
+     560         208 :     ROS_WARN_THROTTLE(10.0, "[ConstraintManager]: can not do constraint management, missing estimation diagnostics!");
+     561         208 :     return;
+     562             :   }
+     563             : 
+     564         936 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     565             : 
+     566         936 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     567             : 
+     568         936 :   mrs_msgs::ConstraintManagerDiagnostics diagnostics;
+     569             : 
+     570         468 :   diagnostics.stamp        = ros::Time::now();
+     571         468 :   diagnostics.current_name = current_constraints;
+     572         468 :   diagnostics.loaded       = _constraint_names_;
+     573             : 
+     574             :   // get the available constraints
+     575             :   {
+     576         468 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     577         468 :     it = _map_type_allowed_constraints_.find(estimation_diagnostics.current_state_estimator);
+     578             : 
+     579         468 :     if (it == _map_type_allowed_constraints_.end()) {
+     580           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: the odometry.type '%s' was not specified in the constraint_manager's config!",
+     581             :                         estimation_diagnostics.current_state_estimator.c_str());
+     582             :     } else {
+     583         468 :       diagnostics.available = it->second;
+     584             :     }
+     585             :   }
+     586             : 
+     587             :   // get the current constraint values
+     588             :   {
+     589         468 :     std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     590         468 :     it = _constraints_.find(current_constraints);
+     591             : 
+     592         468 :     diagnostics.current_values = it->second.constraints;
+     593             :   }
+     594             : 
+     595         468 :   ph_diagnostics_.publish(diagnostics);
+     596             : }
+     597             : 
+     598             : //}
+     599             : 
+     600             : // --------------------------------------------------------------
+     601             : // |                          routines                          |
+     602             : // --------------------------------------------------------------
+     603             : 
+     604             : /* stringInVector() //{ */
+     605             : 
+     606        1134 : bool ConstraintManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     607             : 
+     608        1134 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     609          42 :     return false;
+     610             :   } else {
+     611        1092 :     return true;
+     612             :   }
+     613             : }
+     614             : 
+     615             : //}
+     616             : 
+     617             : }  // namespace constraint_manager
+     618             : 
+     619             : }  // namespace mrs_uav_managers
+     620             : 
+     621             : #include <pluginlib/class_list_macros.h>
+     622          42 : 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..9a046a2692 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html @@ -0,0 +1,176 @@ + + + + + + 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 + + +
+ 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..9ae1d964f15f811ca5d79039c0f6b44997d706ce GIT binary patch literal 2171 zcmV->2!!{EP)!T1s;KO`*+mrIUdn^6rG`EmP#e05rgF98fy{m zr;)5{;LpV{WybbtU595vkTB%~d`dMOj~QYskKa>I99&a;jqf!KF2Z{fLifPBnO3#A zPDYGlgq`jPFWw;`>H`x7fSm7;egnBXKDDe?#r)iM%@ zX!*eI-KNLsj>UX|_I_2{pfIEVH?;{vya^)!FCytOqHINX)f@4rvkDu3s=Rl+1~St!^ZhlR_a3;XdmYz1hqOMon#q(a1IGArMa+0y8@F zdqQh00pTJ=jD)>}aRSK*MLaq_HH1BdIUzd>BUaDaQyBjs>3WqAhC!hr>}Hu&EX*P# zT$DUsX z)>A0?7!$@W*XWN`UAz8?z764udUI=EG|=J+QQQo)g0tA+eYOv7$N6PmKl>5iM1=1` zGlWNpAQ1LH{$x`CWYj<8xEzm9Ij$Y?0z#ybX2(U#8M(W$ST~b+*#{Dqy+`O}?;qhk z`KC*VaFJQ7Qhe4-iJ2hheliWN~*vL&oOBlR- zWp@@Mq9;T@lj=z{#1gI!k=^e}yS^oKh&OwC9yGUN%P4%(DcXudULp<~c(NG3nkGQ%d2BpG=P#AG3x_;yY zve;tr1FmsO5B~>1C`8or*sUl(+tE(E1E!>Er{I^QWxI~Ah`w8#!b|`Qn=AuDb^?IO z%{d)d!rjjYUzq@~McUNkkrqbPdPKC1S0I!k8inuCEXRjwgG}9a4&P~vecum6n7M@~ zL~Ib*Fam{yW=#?C2oaCuaSu-qq|K~)uyzXS!P@z#2uZro-iMIz2!!1f9ub8K@4g_t z%#bIh$j0HymI>4j*Jxk(xP#YAk00SgDr~31tE-JUg{J`|GofiV*w{@ebn4>>mr@Oo zvg`i(c?*s02=9&kz~Fk|M$TUoXl5bfqk$$aEyUHhRmF-U&b!;g*AKU@0I<5VuYyi~)dV#!XBJBXNg+ zI6@ubZ!sZM2hV~cYBuX|sPimD2C4UO3u2}B#POc35C7K*K+0qP#HT#2kD!U4Hm>gi zq~7ED2WB4~Hj*WAxM{yD4i_6c4uwoj^^p`DO@&qA(FMu;B{#^1bo}##!Zxs*4`JfE zUe$Q@T??k3_aL;O!i0a%hDLTf^iQ0e**3O+vG_QCaDHR4e}`M+Ux>`Px|l$ zY*V;lY&Ql+y70oO30}3Vt>#8I02cT25;o3#V_2}#EA+@5V5ilU*Kq+m1%Rfk$4yI# zt?E)jA>zBZXTRI0BZ}Idess9E=&H6y>!5R5K2I1^YZ;^#_Eh@N zWO0LClt4PprsSO!X(U394Ibki$0bou$-{MBCs(MLTN*;kVBXj&NY&g^(2vv7s`8%x zHcZDU4v76;oEz7Y81iCK*!kBE;}pxV&@w2MLi$Y9W41_&X>QvVNs`bwUamcvhDsME zdCu!8m~E`udWzf|{XXB9>nZ4#)bGlx6~*nzoU2zUAd}C)E24{TaQ(|?%EG9U3gn{x zG29;n*MWaa_VGj(mpAI7Xa%AHh&uR=n9LpK-Yi$-e_G+I%fT zDlAC5f5=XoOY8d}4&h!%KPzcxCyZ + + + + + + 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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)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::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::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::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&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&)1
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, double const&)1
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, double const&, double const&)2
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&)42
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&)342
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&)348
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&)349
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&)642
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&)692
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&)737
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&)1637
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&)2507
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&)2771
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)3458
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&)3462
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)3672
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&)21026
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&)30567
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&)31962
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&)35150
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)42108
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&)43305
+
+
+ + + +
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..af8b895b2a --- /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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::idxInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)642
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)3672
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)42108
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)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::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&)43305
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&)1637
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, double const&, double const&)2
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&)1
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&)3458
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, double const&)1
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&)31962
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&)30567
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&)3462
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&)2507
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&)35150
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&)348
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&)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&)42
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&)349
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&)21026
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&)2771
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&)737
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&)342
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&)692
+
+
+ + + +
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..b71f288476 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html @@ -0,0 +1,1301 @@ + + + + + + + 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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_managers/control_manager/common.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : namespace control_manager
+       7             : {
+       8             : 
+       9             : /* idxInVector() //{ */
+      10             : 
+      11         642 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13        1822 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14        1822 :     if (str == vec[i]) {
+      15         642 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           0 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26       30567 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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       30567 :   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           0 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240           0 :   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           0 :   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           0 :   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           0 :   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           0 :   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           0 :   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           0 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277        1637 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281        1637 :   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        1637 :   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        1637 :   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        1637 :   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        1637 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310       43305 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   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       43305 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460           0 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462           0 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464           0 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466           0 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471           0 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472           0 :     return 0.0;
+     473             :   }
+     474             : 
+     475           0 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477           0 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479           0 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483           0 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485           0 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495          42 : 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          84 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500          42 :   if (custom_config != "") {
+     501          42 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504          42 :   if (platform_config != "") {
+     505          42 :     param_loader.addYamlFile(platform_config);
+     506             :   }
+     507             : 
+     508             :   double arm_length;
+     509             :   double body_height;
+     510             :   double force_constant;
+     511             :   double torque_constant;
+     512             :   double prop_radius;
+     513             :   double rpm_min;
+     514             :   double rpm_max;
+     515             :   double mass;
+     516             : 
+     517          42 :   param_loader.loadParam("uav_mass", mass);
+     518             : 
+     519          42 :   param_loader.loadParam("model_params/arm_length", arm_length);
+     520          42 :   param_loader.loadParam("model_params/body_height", body_height);
+     521             : 
+     522          42 :   param_loader.loadParam("model_params/propulsion/force_constant", force_constant);
+     523          42 :   param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant);
+     524          42 :   param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius);
+     525          42 :   param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min);
+     526          42 :   param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max);
+     527             : 
+     528         126 :   Eigen::MatrixXd allocation_matrix = param_loader.loadMatrixDynamic2("model_params/propulsion/allocation_matrix", 4, -1);
+     529             : 
+     530          42 :   if (!param_loader.loadedSuccessfully()) {
+     531           0 :     ROS_INFO("[%s]: detailed model params not loaded, missing some info", node_name.c_str());
+     532           0 :     return {};
+     533             :   }
+     534             : 
+     535          42 :   int n_motors = allocation_matrix.cols();
+     536             : 
+     537          84 :   DetailedModelParams_t model_params;
+     538             : 
+     539          42 :   model_params.arm_length  = arm_length;
+     540          42 :   model_params.body_height = body_height;
+     541          42 :   model_params.prop_radius = prop_radius;
+     542             : 
+     543         126 :   Eigen::MatrixXd inertia_matrix = param_loader.loadMatrixDynamic2("model_params/inertia_matrix", 3, 3);
+     544             : 
+     545          42 :   if (param_loader.loadedSuccessfully()) {
+     546             : 
+     547           0 :     model_params.inertia = inertia_matrix;
+     548           0 :     ROS_INFO("[%s]: inertia loaded from config file:", node_name.c_str());
+     549           0 :     ROS_INFO_STREAM(model_params.inertia);
+     550             : 
+     551             :   } else {
+     552             : 
+     553             :     // create the inertia matrix
+     554          42 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     555          42 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     556          42 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     557          42 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     558             : 
+     559          42 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     560          42 :     ROS_INFO_STREAM(model_params.inertia);
+     561             :   }
+     562             : 
+     563             :   // create the force-torque allocation matrix
+     564          42 :   model_params.force_torque_mixer = allocation_matrix;
+     565          42 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     566          42 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     567          42 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     568          42 :   model_params.force_torque_mixer.row(3) *= force_constant;
+     569             : 
+     570             :   // | ------- create the control group allocation matrix ------- |
+     571             : 
+     572             :   // pseudoinverse of the force-torque matrix (maximum norm)
+     573             :   Eigen::MatrixXd alloc_tmp =
+     574          84 :       model_params.force_torque_mixer.transpose() * (model_params.force_torque_mixer * model_params.force_torque_mixer.transpose()).inverse();
+     575             : 
+     576             :   // | ------------- normalize the allocation matrix ------------ |
+     577             :   // this will make it match the PX4 control group mixing
+     578             : 
+     579             :   // the first two columns (roll, pitch)
+     580         210 :   for (int i = 0; i < n_motors; i++) {
+     581         168 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     582             :   }
+     583             : 
+     584             :   // the 3rd column (yaw)
+     585         210 :   for (int i = 0; i < n_motors; i++) {
+     586         168 :     if (alloc_tmp(i, 2) > 1e-2) {
+     587          84 :       alloc_tmp(i, 2) = 1.0;
+     588          84 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     589          84 :       alloc_tmp(i, 2) = -1.0;
+     590             :     } else {
+     591           0 :       alloc_tmp(i, 2) = 0.0;
+     592             :     }
+     593             :   }
+     594             : 
+     595             :   // the 4th column (throttle)
+     596         210 :   for (int i = 0; i < n_motors; i++) {
+     597         168 :     alloc_tmp(i, 3) = 1.0;
+     598             :   }
+     599             : 
+     600          42 :   std::cout << "Control group mixer: " << std::endl << alloc_tmp << std::endl;
+     601             : 
+     602          42 :   model_params.control_group_mixer = alloc_tmp;
+     603             : 
+     604          42 :   return model_params;
+     605             : }
+     606             : 
+     607             : //}
+     608             : 
+     609             : /* getLowestOutput() //{ */
+     610             : 
+     611        3672 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     612             : 
+     613        3672 :   if (outputs.actuators) {
+     614          12 :     return ACTUATORS_CMD;
+     615             :   }
+     616             : 
+     617        3660 :   if (outputs.control_group) {
+     618          11 :     return CONTROL_GROUP;
+     619             :   }
+     620             : 
+     621        3649 :   if (outputs.attitude_rate) {
+     622        3588 :     return ATTITUDE_RATE;
+     623             :   }
+     624             : 
+     625          61 :   if (outputs.attitude) {
+     626          11 :     return ATTITUDE;
+     627             :   }
+     628             : 
+     629          50 :   if (outputs.acceleration_hdg_rate) {
+     630          10 :     return ACCELERATION_HDG_RATE;
+     631             :   }
+     632             : 
+     633          40 :   if (outputs.acceleration_hdg) {
+     634          10 :     return ACCELERATION_HDG;
+     635             :   }
+     636             : 
+     637          30 :   if (outputs.velocity_hdg_rate) {
+     638          10 :     return VELOCITY_HDG_RATE;
+     639             :   }
+     640             : 
+     641          20 :   if (outputs.velocity_hdg) {
+     642          10 :     return VELOCITY_HDG;
+     643             :   }
+     644             : 
+     645          10 :   return POSITION;
+     646             : }
+     647             : 
+     648             : //}
+     649             : 
+     650             : /* getHighestOutput() //{ */
+     651             : 
+     652           0 : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs) {
+     653             : 
+     654           0 :   if (outputs.position) {
+     655           0 :     return POSITION;
+     656             :   }
+     657             : 
+     658           0 :   if (outputs.velocity_hdg) {
+     659           0 :     return VELOCITY_HDG;
+     660             :   }
+     661             : 
+     662           0 :   if (outputs.velocity_hdg_rate) {
+     663           0 :     return VELOCITY_HDG_RATE;
+     664             :   }
+     665             : 
+     666           0 :   if (outputs.acceleration_hdg) {
+     667           0 :     return ACCELERATION_HDG;
+     668             :   }
+     669             : 
+     670           0 :   if (outputs.acceleration_hdg_rate) {
+     671           0 :     return ACCELERATION_HDG_RATE;
+     672             :   }
+     673             : 
+     674           0 :   if (outputs.attitude) {
+     675           0 :     return ATTITUDE;
+     676             :   }
+     677             : 
+     678           0 :   if (outputs.attitude_rate) {
+     679           0 :     return ATTITUDE_RATE;
+     680             :   }
+     681             : 
+     682           0 :   if (outputs.control_group) {
+     683           0 :     return CONTROL_GROUP;
+     684             :   }
+     685             : 
+     686           0 :   return ACTUATORS_CMD;
+     687             : }
+     688             : 
+     689             : //}
+     690             : 
+     691             : // | -------- extraction of throttle out of hw api cmds ------- |
+     692             : 
+     693             : /* extractThrottle() //{ */
+     694             : 
+     695       42108 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     696             : 
+     697       42108 :   if (!control_output.control_output) {
+     698        5239 :     return {};
+     699             :   }
+     700             : 
+     701       73738 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     702             : }
+     703             : 
+     704             : //}
+     705             : 
+     706             : // | -------------- validation of HW api commands ------------- |
+     707             : 
+     708             : /* validateControlOutput() //{ */
+     709             : 
+     710       31962 : bool validateControlOutput(const Controller::ControlOutput& control_output, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     711             :                            const std::string& var_name) {
+     712             : 
+     713       31962 :   if (!control_output.control_output) {
+     714           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: the optional variable '%s' is not set!!!", node_name.c_str(), var_name.c_str());
+     715           0 :     return false;
+     716             :   }
+     717             : 
+     718       31962 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     719       63924 :   std::variant<std::string>               node_name_var{node_name};
+     720       31962 :   std::variant<std::string>               var_name_var{var_name};
+     721             : 
+     722       31962 :   return std::visit(HwApiValidateVisitor(), control_output.control_output.value(), output_modalities_var, node_name_var, var_name_var);
+     723             : }
+     724             : 
+     725             : //}
+     726             : 
+     727             : /* validateHwApiActuatorCmd() //{ */
+     728             : 
+     729        2507 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     730             : 
+     731       12535 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     732       10028 :     if (!std::isfinite(msg.motors[i])) {
+     733           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.motors[%d]'!!!", node_name.c_str(), var_name.c_str(), int(i));
+     734           0 :       return false;
+     735             :     }
+     736             :   }
+     737             : 
+     738        2507 :   return true;
+     739             : }
+     740             : 
+     741             : //}
+     742             : 
+     743             : /* validateHwApiControlGroupCmd() //{ */
+     744             : 
+     745        2771 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     746             : 
+     747        2771 :   if (!std::isfinite(msg.roll)) {
+     748           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.roll'!!!", node_name.c_str(), var_name.c_str());
+     749           0 :     return false;
+     750             :   }
+     751             : 
+     752        2771 :   if (!std::isfinite(msg.pitch)) {
+     753           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pitch'!!!", node_name.c_str(), var_name.c_str());
+     754           0 :     return false;
+     755             :   }
+     756             : 
+     757        2771 :   if (!std::isfinite(msg.yaw)) {
+     758           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.yaw'!!!", node_name.c_str(), var_name.c_str());
+     759           0 :     return false;
+     760             :   }
+     761             : 
+     762        2771 :   if (!std::isfinite(msg.throttle)) {
+     763           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     764           0 :     return false;
+     765             :   }
+     766             : 
+     767        2771 :   return true;
+     768             : }
+     769             : 
+     770             : //}
+     771             : 
+     772             : /* validateHwApiAttitudeCmd() //{ */
+     773             : 
+     774       35150 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     775             : 
+     776             :   // check the orientation
+     777             : 
+     778       35150 :   if (!std::isfinite(msg.orientation.x)) {
+     779           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     780           0 :     return false;
+     781             :   }
+     782             : 
+     783       35150 :   if (!std::isfinite(msg.orientation.y)) {
+     784           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     785           0 :     return false;
+     786             :   }
+     787             : 
+     788       35150 :   if (!std::isfinite(msg.orientation.z)) {
+     789           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     790           0 :     return false;
+     791             :   }
+     792             : 
+     793       35150 :   if (!std::isfinite(msg.orientation.w)) {
+     794           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     795           0 :     return false;
+     796             :   }
+     797             : 
+     798             :   // check the throttle
+     799             : 
+     800       35150 :   if (!std::isfinite(msg.throttle)) {
+     801           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     802           0 :     return false;
+     803             :   }
+     804             : 
+     805       35150 :   return true;
+     806             : }
+     807             : 
+     808             : //}
+     809             : 
+     810             : /* validateHwApiAttitudeRateCmd() //{ */
+     811             : 
+     812       21026 : bool validateHwApiAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     813             : 
+     814             :   // check the body rate
+     815             : 
+     816       21026 :   if (!std::isfinite(msg.body_rate.x)) {
+     817           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.x'!!!", node_name.c_str(), var_name.c_str());
+     818           0 :     return false;
+     819             :   }
+     820             : 
+     821       21026 :   if (!std::isfinite(msg.body_rate.y)) {
+     822           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.y'!!!", node_name.c_str(), var_name.c_str());
+     823           0 :     return false;
+     824             :   }
+     825             : 
+     826       21026 :   if (!std::isfinite(msg.body_rate.z)) {
+     827           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.z'!!!", node_name.c_str(), var_name.c_str());
+     828           0 :     return false;
+     829             :   }
+     830             : 
+     831             :   // check the throttle
+     832             : 
+     833       21026 :   if (!std::isfinite(msg.throttle)) {
+     834           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     835           0 :     return false;
+     836             :   }
+     837             : 
+     838       21026 :   return true;
+     839             : }
+     840             : 
+     841             : //}
+     842             : 
+     843             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     844             : 
+     845         692 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     846             : 
+     847             :   // | ----------------- check the acceleration ----------------- |
+     848             : 
+     849         692 :   if (!std::isfinite(msg.acceleration.x)) {
+     850           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     851           0 :     return false;
+     852             :   }
+     853             : 
+     854         692 :   if (!std::isfinite(msg.acceleration.y)) {
+     855           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     856           0 :     return false;
+     857             :   }
+     858             : 
+     859         692 :   if (!std::isfinite(msg.acceleration.z)) {
+     860           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     861           0 :     return false;
+     862             :   }
+     863             : 
+     864             :   // check the heading rate
+     865             : 
+     866         692 :   if (!std::isfinite(msg.heading_rate)) {
+     867           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     868           0 :     return false;
+     869             :   }
+     870             : 
+     871         692 :   return true;
+     872             : }
+     873             : 
+     874             : //}
+     875             : 
+     876             : /* validateHwApiAccelerationHdgCmd() //{ */
+     877             : 
+     878         737 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     879             : 
+     880             :   // | ----------------- check the acceleration ----------------- |
+     881             : 
+     882         737 :   if (!std::isfinite(msg.acceleration.x)) {
+     883           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     884           0 :     return false;
+     885             :   }
+     886             : 
+     887         737 :   if (!std::isfinite(msg.acceleration.y)) {
+     888           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     889           0 :     return false;
+     890             :   }
+     891             : 
+     892         737 :   if (!std::isfinite(msg.acceleration.z)) {
+     893           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     894           0 :     return false;
+     895             :   }
+     896             : 
+     897             :   // check the heading
+     898             : 
+     899         737 :   if (!std::isfinite(msg.heading)) {
+     900           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     901           0 :     return false;
+     902             :   }
+     903             : 
+     904         737 :   return true;
+     905             : }
+     906             : 
+     907             : //}
+     908             : 
+     909             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     910             : 
+     911         342 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     912             : 
+     913             :   // | ----------------- check the velocity ----------------- |
+     914             : 
+     915         342 :   if (!std::isfinite(msg.velocity.x)) {
+     916           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     917           0 :     return false;
+     918             :   }
+     919             : 
+     920         342 :   if (!std::isfinite(msg.velocity.y)) {
+     921           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     922           0 :     return false;
+     923             :   }
+     924             : 
+     925         342 :   if (!std::isfinite(msg.velocity.z)) {
+     926           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     927           0 :     return false;
+     928             :   }
+     929             : 
+     930             :   // check the heading rate
+     931             : 
+     932         342 :   if (!std::isfinite(msg.heading_rate)) {
+     933           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     934           0 :     return false;
+     935             :   }
+     936             : 
+     937         342 :   return true;
+     938             : }
+     939             : 
+     940             : //}
+     941             : 
+     942             : /* validateHwApiVelocityHdgCmd() //{ */
+     943             : 
+     944         349 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     945             : 
+     946             :   // | ----------------- check the velocity ----------------- |
+     947             : 
+     948         349 :   if (!std::isfinite(msg.velocity.x)) {
+     949           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     950           0 :     return false;
+     951             :   }
+     952             : 
+     953         349 :   if (!std::isfinite(msg.velocity.y)) {
+     954           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     955           0 :     return false;
+     956             :   }
+     957             : 
+     958         349 :   if (!std::isfinite(msg.velocity.z)) {
+     959           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     960           0 :     return false;
+     961             :   }
+     962             : 
+     963             :   // check the heading
+     964             : 
+     965         349 :   if (!std::isfinite(msg.heading)) {
+     966           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     967           0 :     return false;
+     968             :   }
+     969             : 
+     970         349 :   return true;
+     971             : }
+     972             : 
+     973             : //}
+     974             : 
+     975             : /* validateHwApiPositionHdgCmd() //{ */
+     976             : 
+     977         348 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     978             : 
+     979             :   // | ----------------- check the position ----------------- |
+     980             : 
+     981         348 :   if (!std::isfinite(msg.position.x)) {
+     982           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.x'!!!", node_name.c_str(), var_name.c_str());
+     983           0 :     return false;
+     984             :   }
+     985             : 
+     986         348 :   if (!std::isfinite(msg.position.y)) {
+     987           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.y'!!!", node_name.c_str(), var_name.c_str());
+     988           0 :     return false;
+     989             :   }
+     990             : 
+     991         348 :   if (!std::isfinite(msg.position.z)) {
+     992           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.z'!!!", node_name.c_str(), var_name.c_str());
+     993           0 :     return false;
+     994             :   }
+     995             : 
+     996             :   // check the heading
+     997             : 
+     998         348 :   if (!std::isfinite(msg.heading)) {
+     999           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+    1000           0 :     return false;
+    1001             :   }
+    1002             : 
+    1003         348 :   return true;
+    1004             : }
+    1005             : 
+    1006             : //}
+    1007             : 
+    1008             : // | ------------ initialization of HW api commands ----------- |
+    1009             : 
+    1010             : /* initializeDefaultOutput() //{ */
+    1011             : 
+    1012        3462 : Controller::HwApiOutputVariant initializeDefaultOutput(const ControlOutputModalities_t& possible_outputs, const mrs_msgs::UavState& uav_state,
+    1013             :                                                        const double& min_throttle, const double& n_motors) {
+    1014             : 
+    1015        3462 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1016             : 
+    1017        3462 :   Controller::HwApiOutputVariant output;
+    1018             : 
+    1019        3462 :   switch (lowest_output) {
+    1020           2 :     case ACTUATORS_CMD: {
+    1021           2 :       output = mrs_msgs::HwApiActuatorCmd();
+    1022           2 :       break;
+    1023             :     }
+    1024           1 :     case CONTROL_GROUP: {
+    1025           1 :       output = mrs_msgs::HwApiControlGroupCmd();
+    1026           1 :       break;
+    1027             :     }
+    1028        3458 :     case ATTITUDE_RATE: {
+    1029        3458 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1030        3458 :       break;
+    1031             :     }
+    1032           1 :     case ATTITUDE: {
+    1033           1 :       output = mrs_msgs::HwApiAttitudeCmd();
+    1034           1 :       break;
+    1035             :     }
+    1036           0 :     case ACCELERATION_HDG_RATE: {
+    1037           0 :       output = mrs_msgs::HwApiAccelerationHdgRateCmd();
+    1038           0 :       break;
+    1039             :     }
+    1040           0 :     case ACCELERATION_HDG: {
+    1041           0 :       output = mrs_msgs::HwApiAccelerationHdgCmd();
+    1042           0 :       break;
+    1043             :     }
+    1044           0 :     case VELOCITY_HDG_RATE: {
+    1045           0 :       output = mrs_msgs::HwApiVelocityHdgRateCmd();
+    1046           0 :       break;
+    1047             :     }
+    1048           0 :     case VELOCITY_HDG: {
+    1049           0 :       output = mrs_msgs::HwApiVelocityHdgCmd();
+    1050           0 :       break;
+    1051             :     }
+    1052           0 :     case POSITION: {
+    1053           0 :       output = mrs_msgs::HwApiPositionCmd();
+    1054           0 :       break;
+    1055             :     }
+    1056             :   }
+    1057             : 
+    1058        6924 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1059        3462 :   std::variant<double>             min_throttle_var{min_throttle};
+    1060        3462 :   std::variant<double>             n_motors_var{n_motors};
+    1061             : 
+    1062        3462 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1063             : 
+    1064        6924 :   return output;
+    1065             : }  // namespace mrs_uav_managers
+    1066             : 
+    1067             : //}
+    1068             : 
+    1069             : /* initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg) //{ */
+    1070             : 
+    1071           2 : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors) {
+    1072             : 
+    1073           2 :   msg.stamp = ros::Time::now();
+    1074             : 
+    1075          10 :   for (int i = 0; i < n_motors; i++) {
+    1076           8 :     msg.motors.push_back(min_throttle);
+    1077             :   }
+    1078           2 : }
+    1079             : 
+    1080             : //}
+    1081             : 
+    1082             : /* initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg) //{ */
+    1083             : 
+    1084           1 : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle) {
+    1085             : 
+    1086           1 :   msg.stamp = ros::Time::now();
+    1087             : 
+    1088           1 :   msg.roll     = 0;
+    1089           1 :   msg.pitch    = 0;
+    1090           1 :   msg.yaw      = 0;
+    1091           1 :   msg.throttle = min_throttle;
+    1092           1 : }
+    1093             : 
+    1094             : //}
+    1095             : 
+    1096             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg) //{ */
+    1097             : 
+    1098        3458 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1099             : 
+    1100        3458 :   msg.stamp = ros::Time::now();
+    1101             : 
+    1102        3458 :   msg.body_rate.x = 0;
+    1103        3458 :   msg.body_rate.y = 0;
+    1104        3458 :   msg.body_rate.z = 0;
+    1105        3458 :   msg.throttle    = min_throttle;
+    1106        3458 : }
+    1107             : 
+    1108             : //}
+    1109             : 
+    1110             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg) //{ */
+    1111             : 
+    1112           1 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle) {
+    1113             : 
+    1114           1 :   msg.stamp = ros::Time::now();
+    1115             : 
+    1116           1 :   msg.orientation = uav_state.pose.orientation;
+    1117           1 :   msg.throttle    = min_throttle;
+    1118           1 : }
+    1119             : 
+    1120             : //}
+    1121             : 
+    1122             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg) //{ */
+    1123             : 
+    1124           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1125             : 
+    1126           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1127           0 :   msg.header.stamp    = ros::Time::now();
+    1128             : 
+    1129           0 :   msg.acceleration.x = 0;
+    1130           0 :   msg.acceleration.y = 0;
+    1131           0 :   msg.acceleration.z = 0;
+    1132           0 :   msg.heading_rate   = 0;
+    1133           0 : }
+    1134             : 
+    1135             : //}
+    1136             : 
+    1137             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg) //{ */
+    1138             : 
+    1139           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1140             : 
+    1141           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1142           0 :   msg.header.stamp    = ros::Time::now();
+    1143             : 
+    1144           0 :   msg.acceleration.x = 0;
+    1145           0 :   msg.acceleration.y = 0;
+    1146           0 :   msg.acceleration.z = 0;
+    1147             : 
+    1148             :   try {
+    1149           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1150             :   }
+    1151           0 :   catch (std::runtime_error& exrun) {
+    1152           0 :     msg.heading = 0;
+    1153             :   }
+    1154           0 : }
+    1155             : 
+    1156             : //}
+    1157             : 
+    1158             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg) //{ */
+    1159             : 
+    1160           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1161             : 
+    1162           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1163           0 :   msg.header.stamp    = ros::Time::now();
+    1164             : 
+    1165           0 :   msg.velocity.x   = 0;
+    1166           0 :   msg.velocity.y   = 0;
+    1167           0 :   msg.velocity.z   = 0;
+    1168           0 :   msg.heading_rate = 0;
+    1169           0 : }
+    1170             : 
+    1171             : //}
+    1172             : 
+    1173             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg) //{ */
+    1174             : 
+    1175           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1176             : 
+    1177           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1178           0 :   msg.header.stamp    = ros::Time::now();
+    1179             : 
+    1180           0 :   msg.velocity.x = 0;
+    1181           0 :   msg.velocity.y = 0;
+    1182           0 :   msg.velocity.z = 0;
+    1183             : 
+    1184             :   try {
+    1185           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1186             :   }
+    1187           0 :   catch (std::runtime_error& exrun) {
+    1188           0 :     msg.heading = 0;
+    1189             :   }
+    1190           0 : }
+    1191             : 
+    1192             : //}
+    1193             : 
+    1194             : /* initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg) //{ */
+    1195             : 
+    1196           0 : void initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1197             : 
+    1198           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1199           0 :   msg.header.stamp    = ros::Time::now();
+    1200             : 
+    1201           0 :   msg.position.x = uav_state.pose.position.x;
+    1202           0 :   msg.position.y = uav_state.pose.position.y;
+    1203           0 :   msg.position.z = uav_state.pose.position.z;
+    1204             : 
+    1205             :   try {
+    1206           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1207             :   }
+    1208           0 :   catch (std::runtime_error& exrun) {
+    1209           0 :     msg.heading = 0;
+    1210             :   }
+    1211           0 : }
+    1212             : 
+    1213             : //}
+    1214             : 
+    1215             : }  // namespace control_manager
+    1216             : 
+    1217             : }  // 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..d11dc2de30 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html @@ -0,0 +1,325 @@ + + + + + + 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 + + +
+ 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..b94effe9f71fa9bae81d29f64162ea051aa6fb9a GIT binary patch literal 2387 zcmV-Z39R;sP)^jkqU&=u+icK=mQ`Tq$J6fAh*p6AZ?(vUZV00H9bZ!g0iR9+{` z@!L0s$@^>#sH&sfD^x9%IQ$KTm@m3a%Pb#f;~W0`8j5ss{a zgJ6nt+$kB0WYuGeL=cv5n$P$9UB91Xd2MMLPKYjXBRo^AzWUz(T;{ke(_)r+0m|OO zIcBzV^o$B1{2Waq%f^~%6;J!arap4Q{W;Y_uMe^fGc`V3@rQ019fgNFSy(+vNxb}0 zJp`szFxqfSIiW|XhrqN7MjLJ^p9`o5R8SY{Auz3i5r$iE&gZEoFdIjz=eIK{0@ErO zGF$@FB^WhTcvqb&0BhMU1|$O0Di~$B89&KXPapt;RnOX5TSI=Ti?#+07XZ{Pm~LA} z4<8x7$y5(Kykga}{kE-HY#0P!fx(6LkKjJG)C`;#iJI?)HH+DV>S>k#wFwKywoGw* z{FpKt(z>?NMRz4It%5N^b{Ri9p`IOJhw33P1m*@XG<*=4PQkF6(;T}yp;6>K(AR5g z`?H*Jwl&f8Op#aJhdmOQPQbue&|rQV6kwN`D zvk#Me&N!KRM1;k(ezY-13fp8C-Ic(!3Z{Gr z8QypG$C@tGLtqHZ4Paz1d>8A7uzGwi)rEQpOsinj@KJ2;^QeAJU|I!ZdE6pC_wnb= z2uurLERS2n=Z4hd_UtayLtt73W2`jaDqqAU^9`xTRZ!xcDanne2s0No$_a*rkfJV2)96Fsp@~83_AkGYIoIGJ8!B zB00YHp>oJ*afBXi)OHX1YOBvjjp#F2)adeuy!l-rV_NN_mj(PYF!X{0a&m)KC2K>M z(XW%UaMxVOF>};&iX*dIB(K|RH0ss`B18_@IjA+*n2-S(eIYBz_(D&<(a{k_nBr*= z#(Cyeo$id8gh?`d@tb<`x`8kXWLv|=qq^9VCX{wsu4;#InsCMu3bHv-HBSwRkkKN{ zMXmE#f_VnBl3Dt$7&=I%S4f+fShGp>CcfCzBwG%{i1E6@3W=#>vLJz|a}{nH08Ds- z<1!0LlY|lyRCxwru8gzIw6K|h7v9kv%@wKOyGVRw+tMlB6u&~_nal}egp`N-CgGFBqAHJsj5;XD+hptoRy z<}^+7EYezsl5H5;ndZebESIoK=Nbfxs5)}MOcKi+$+N7DVv>F6A3Gs#&Q&S3`=EMgWL=YOkBNqR zpw8fEudV`weQg(%dNY7jv-a|~`WMlC&&_dTz4kR4O%qdo)xc0(N|F&RV>Cs-A!DNI z+IkG@($Zs0m-S_g4a264k*e$KF=UaBjIn{eu8a|jv}BA{U1!il^n~69q=Ng`Un?eI zZD)(HTUPkNxJ(|n;IF2IvbGe*x)T<5Tx5Th!7Y8a|0OkJ*4yMp;5skM3?__>Z+CVhi`|`{&+dp%n{Z`IDTiX0Up>$y}H>2I#OVgN{+OcJrbBU!5EG-<40NQp%)04gy=$!RIioJyU~Ze?}Na! z3WjE+2`~?Kq}LB$0bbMJO3U3jBS%VL`U68V(gYZCq>)nP2m(1$0@EKDa-<0`k8-4T zz|py0aP@XUk?RFc&vH#kJ;hwJxN9J??V!tOSK_DTl1+$gYCrA7w1l#e<#3d8C9@#N z%lnBd^dkT^NQjXUdCBLhjJ`1cyNro!Hot7^!-r$;BFk$C?p;>)BY+U$?9ulXEE(hZ z!vivIAYnBb<2SlnkTI6SJ1C=9O-&i25(Z@q5|)&)&ixs0L;@UvNyxu({ThnUaVa=Y z0o;qzqq&irHGj}d63#NsRM0aDLUg(3#E_WOot%G#?#~*pc+S2zyqJ0YfqfwkUi=a_BX7O92V10DA3RKld!Wqp`Po=_yWIat=4`=!#LY9Ft zhNf%lsX8FB(unm~5{3w$-PSVxlYbAy$;0IqmQ7sTPQ{g%jw)`O#Vbho`RHgPJYmnS zdR^n$cIpv+5`^e%J4m|E&1QDS$P#9zjEu}INr;v4g$*Z*T>wXke!&O#Ki1~T8Vr*d zRi`*&tESHNn9h~(qcsT=ov?Q^J7;83Gc$~=Gc!vxbrnLajO4@P9I=Ewths`ni@$qH z{by&L%R+JsAy!7}Tyc(A!t$N#%@cgeQxXOEI~;>!-# + + + + + + 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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
<unnamed>41.9 %236 / 56371.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html b/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html new file mode 100644 index 0000000000..dea7aa024e --- /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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
<unnamed>41.9 %236 / 56371.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-detail.html b/mrs_uav_managers/src/control_manager/common/index-detail.html new file mode 100644 index 0000000000..2cb9f619da --- /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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
<unnamed>41.9 %236 / 56371.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-sort-f.html b/mrs_uav_managers/src/control_manager/common/index-sort-f.html new file mode 100644 index 0000000000..635cc0becc --- /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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-sort-l.html b/mrs_uav_managers/src/control_manager/common/index-sort-l.html new file mode 100644 index 0000000000..66df72e158 --- /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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index.html b/mrs_uav_managers/src/control_manager/common/index.html new file mode 100644 index 0000000000..12422ad1b8 --- /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:23656341.9 %
Date:2023-12-06 07:59:45Functions:223171.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.9%41.9%
+ 		41.9 %236 / 56371.0 %22 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..d77fecd70d --- /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:1858366950.6 %
Date:2023-12-06 07:59:45Functions:6111155.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)0
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::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)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::bumperGetSectorId(double const&, double const&, double 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::escalatingFailsafe[abi:cxx11]()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::gotoTrajectoryStart[abi:cxx11]()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::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)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::bumperPushFromObstacle()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::getNextEscFailsafeState()0
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[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::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<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::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)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::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)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::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::ehover[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::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)1
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)2
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()3
mrs_uav_managers::control_manager::ControlManager::elandSrv()3
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)4
mrs_uav_managers::control_manager::ControlManager::isOffboard()6
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)6
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)6
mrs_uav_managers::control_manager::ControlManager::shutdown()9
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)27
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)30
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)30
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::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)39
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::control_manager::ControlManager::initialize()42
mrs_uav_managers::control_manager::ControlManager::preinitialize()42
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)42
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)42
mrs_uav_managers::control_manager::ControlManager::onInit()42
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)48
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)68
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)80
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)82
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()87
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)89
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)91
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)123
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)126
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)162
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)210
mrs_uav_managers::control_manager::ControlManager::getMass()214
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)234
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)253
mrs_uav_managers::control_manager::ControlManager::ungripSrv()283
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)1395
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)1567
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1668
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3580
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)4862
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()4901
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()4901
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)5850
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)18481
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)25631
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)29899
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)39041
mrs_uav_managers::control_manager::ControlManager::updateTrackers()39318
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)40713
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)40713
mrs_uav_managers::control_manager::ControlManager::publish()40713
mrs_uav_managers::control_manager::ControlManager::asyncControl()40744
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)46402
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)95245
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)195281
+
+
+ + + +
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..67eac48a15 --- /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:1858366950.6 %
Date:2023-12-06 07:59:45Functions:6111155.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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)253
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)5850
mrs_uav_managers::control_manager::ControlManager::initialize()42
mrs_uav_managers::control_manager::ControlManager::isOffboard()6
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)234
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)95245
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)4862
mrs_uav_managers::control_manager::ControlManager::asyncControl()40744
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)29899
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)27
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)2
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)30
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)48
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()42
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)91
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)1395
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)18481
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&)42
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()39318
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::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)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::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)46402
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()4901
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)89
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double 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::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)40713
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)6
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()4901
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)195281
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]()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::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)39
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)4
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)82
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&)1668
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)126
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()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::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)42
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)1
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)68
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()0
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()87
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[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::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)80
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&)123
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> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<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::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)1567
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&)30
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)40713
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)162
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)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::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)39041
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> >&)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::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()3
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)6
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::onInit()42
mrs_uav_managers::control_manager::ControlManager::getMass()214
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3580
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)25631
mrs_uav_managers::control_manager::ControlManager::publish()40713
mrs_uav_managers::control_manager::ControlManager::elandSrv()3
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::shutdown()9
mrs_uav_managers::control_manager::ControlManager::ungripSrv()283
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)210
+
+
+ + + +
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..3b6c513529 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,8819 @@ + + + + + + + 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:1858366950.6 %
Date:2023-12-06 07:59:45Functions:6111155.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_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/ObstacleSectors.h>
+      16             : #include <mrs_msgs/BoolStamped.h>
+      17             : #include <mrs_msgs/BumperStatus.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/BumperParamsSrv.h>
+      25             : #include <mrs_msgs/TrackerCommand.h>
+      26             : #include <mrs_msgs/EstimatorInput.h>
+      27             : 
+      28             : #include <geometry_msgs/Point32.h>
+      29             : #include <geometry_msgs/TwistStamped.h>
+      30             : #include <geometry_msgs/PoseArray.h>
+      31             : #include <geometry_msgs/Vector3Stamped.h>
+      32             : 
+      33             : #include <nav_msgs/Odometry.h>
+      34             : 
+      35             : #include <sensor_msgs/Joy.h>
+      36             : #include <sensor_msgs/NavSatFix.h>
+      37             : 
+      38             : #include <mrs_lib/safety_zone/safety_zone.h>
+      39             : #include <mrs_lib/profiler.h>
+      40             : #include <mrs_lib/param_loader.h>
+      41             : #include <mrs_lib/utils.h>
+      42             : #include <mrs_lib/mutex.h>
+      43             : #include <mrs_lib/transformer.h>
+      44             : #include <mrs_lib/geometry/misc.h>
+      45             : #include <mrs_lib/geometry/cyclic.h>
+      46             : #include <mrs_lib/attitude_converter.h>
+      47             : #include <mrs_lib/subscribe_handler.h>
+      48             : #include <mrs_lib/msg_extractor.h>
+      49             : #include <mrs_lib/quadratic_throttle_model.h>
+      50             : #include <mrs_lib/publisher_handler.h>
+      51             : #include <mrs_lib/service_client_handler.h>
+      52             : 
+      53             : #include <mrs_msgs/HwApiCapabilities.h>
+      54             : #include <mrs_msgs/HwApiStatus.h>
+      55             : #include <mrs_msgs/HwApiRcChannels.h>
+      56             : 
+      57             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      58             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      59             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      60             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      61             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      62             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      63             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      64             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      65             : #include <mrs_msgs/HwApiPositionCmd.h>
+      66             : 
+      67             : #include <std_msgs/Float64.h>
+      68             : 
+      69             : #include <future>
+      70             : 
+      71             : #include <pluginlib/class_loader.h>
+      72             : 
+      73             : #include <nodelet/loader.h>
+      74             : 
+      75             : #include <eigen3/Eigen/Eigen>
+      76             : 
+      77             : #include <visualization_msgs/Marker.h>
+      78             : #include <visualization_msgs/MarkerArray.h>
+      79             : 
+      80             : #include <mrs_msgs/Reference.h>
+      81             : #include <mrs_msgs/ReferenceStamped.h>
+      82             : #include <mrs_msgs/ReferenceList.h>
+      83             : #include <mrs_msgs/TrajectoryReference.h>
+      84             : 
+      85             : #include <mrs_msgs/ReferenceStampedSrv.h>
+      86             : #include <mrs_msgs/ReferenceStampedSrvRequest.h>
+      87             : #include <mrs_msgs/ReferenceStampedSrvResponse.h>
+      88             : 
+      89             : #include <mrs_msgs/VelocityReferenceStampedSrv.h>
+      90             : #include <mrs_msgs/VelocityReferenceStampedSrvRequest.h>
+      91             : #include <mrs_msgs/VelocityReferenceStampedSrvResponse.h>
+      92             : 
+      93             : #include <mrs_msgs/TransformReferenceSrv.h>
+      94             : #include <mrs_msgs/TransformReferenceSrvRequest.h>
+      95             : #include <mrs_msgs/TransformReferenceSrvResponse.h>
+      96             : 
+      97             : #include <mrs_msgs/TransformPoseSrv.h>
+      98             : #include <mrs_msgs/TransformPoseSrvRequest.h>
+      99             : #include <mrs_msgs/TransformPoseSrvResponse.h>
+     100             : 
+     101             : #include <mrs_msgs/TransformVector3Srv.h>
+     102             : #include <mrs_msgs/TransformVector3SrvRequest.h>
+     103             : #include <mrs_msgs/TransformVector3SrvResponse.h>
+     104             : 
+     105             : #include <mrs_msgs/Float64StampedSrv.h>
+     106             : #include <mrs_msgs/Float64StampedSrvRequest.h>
+     107             : #include <mrs_msgs/Float64StampedSrvResponse.h>
+     108             : 
+     109             : #include <mrs_msgs/Vec4.h>
+     110             : #include <mrs_msgs/Vec4Request.h>
+     111             : #include <mrs_msgs/Vec4Response.h>
+     112             : 
+     113             : #include <mrs_msgs/Vec1.h>
+     114             : #include <mrs_msgs/Vec1Request.h>
+     115             : #include <mrs_msgs/Vec1Response.h>
+     116             : 
+     117             : //}
+     118             : 
+     119             : /* defines //{ */
+     120             : 
+     121             : #define TAU 2 * M_PI
+     122             : #define REF_X 0
+     123             : #define REF_Y 1
+     124             : #define REF_Z 2
+     125             : #define REF_HEADING 3
+     126             : #define ELAND_STR "eland"
+     127             : #define EHOVER_STR "ehover"
+     128             : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+     129             : #define FAILSAFE_STR "failsafe"
+     130             : #define INPUT_UAV_STATE 0
+     131             : #define INPUT_ODOMETRY 1
+     132             : #define RC_DEADBAND 0.2
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* using //{ */
+     137             : 
+     138             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+     139             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+     140             : 
+     141             : using radians  = mrs_lib::geometry::radians;
+     142             : using sradians = mrs_lib::geometry::sradians;
+     143             : 
+     144             : //}
+     145             : 
+     146             : namespace mrs_uav_managers
+     147             : {
+     148             : 
+     149             : namespace control_manager
+     150             : {
+     151             : 
+     152             : /* //{ class ControlManager */
+     153             : 
+     154             : // state machine
+     155             : typedef enum
+     156             : {
+     157             : 
+     158             :   IDLE_STATE,
+     159             :   LANDING_STATE,
+     160             : 
+     161             : } LandingStates_t;
+     162             : 
+     163             : const char* state_names[2] = {
+     164             : 
+     165             :     "IDLING", "LANDING"};
+     166             : 
+     167             : // state machine
+     168             : typedef enum
+     169             : {
+     170             : 
+     171             :   FCU_FRAME,
+     172             :   RELATIVE_FRAME,
+     173             :   ABSOLUTE_FRAME
+     174             : 
+     175             : } ReferenceFrameType_t;
+     176             : 
+     177             : // state machine
+     178             : typedef enum
+     179             : {
+     180             : 
+     181             :   ESC_NONE_STATE     = 0,
+     182             :   ESC_EHOVER_STATE   = 1,
+     183             :   ESC_ELAND_STATE    = 2,
+     184             :   ESC_FAILSAFE_STATE = 3,
+     185             :   ESC_FINISHED_STATE = 4,
+     186             : 
+     187             : } EscalatingFailsafeStates_t;
+     188             : 
+     189             : /* class ControllerParams() //{ */
+     190             : 
+     191             : class ControllerParams {
+     192             : 
+     193             : public:
+     194             :   ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+     195             :                    bool human_switchable);
+     196             : 
+     197             : public:
+     198             :   double      failsafe_threshold;
+     199             :   double      eland_threshold;
+     200             :   double      odometry_innovation_threshold;
+     201             :   std::string address;
+     202             :   std::string name_space;
+     203             :   bool        human_switchable;
+     204             : };
+     205             : 
+     206         210 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     207         210 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     208             : 
+     209         210 :   this->eland_threshold               = eland_threshold;
+     210         210 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     211         210 :   this->failsafe_threshold            = failsafe_threshold;
+     212         210 :   this->address                       = address;
+     213         210 :   this->name_space                    = name_space;
+     214         210 :   this->human_switchable              = human_switchable;
+     215         210 : }
+     216             : 
+     217             : //}
+     218             : 
+     219             : /* class TrackerParams() //{ */
+     220             : 
+     221             : class TrackerParams {
+     222             : 
+     223             : public:
+     224             :   TrackerParams(std::string address, std::string name_space, bool human_switchable);
+     225             : 
+     226             : public:
+     227             :   std::string address;
+     228             :   std::string name_space;
+     229             :   bool        human_switchable;
+     230             : };
+     231             : 
+     232         253 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     233             : 
+     234         253 :   this->address          = address;
+     235         253 :   this->name_space       = name_space;
+     236         253 :   this->human_switchable = human_switchable;
+     237         253 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : class ControlManager : public nodelet::Nodelet {
+     242             : 
+     243             : public:
+     244             :   virtual void onInit();
+     245             : 
+     246             : private:
+     247             :   ros::NodeHandle   nh_;
+     248             :   std::atomic<bool> is_initialized_ = false;
+     249             :   std::string       _uav_name_;
+     250             :   std::string       _body_frame_;
+     251             : 
+     252             :   std::string _custom_config_;
+     253             :   std::string _platform_config_;
+     254             :   std::string _world_config_;
+     255             :   std::string _network_config_;
+     256             : 
+     257             :   // | --------------- dynamic loading of trackers -------------- |
+     258             : 
+     259             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     260             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     261             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     262             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     263             :   std::mutex                                                         mutex_tracker_list_;
+     264             : 
+     265             :   // | ------------- dynamic loading of controllers ------------- |
+     266             : 
+     267             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     268             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     269             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     270             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     271             :   std::mutex                                                            mutex_controller_list_;
+     272             : 
+     273             :   // | ------------------------- HW API ------------------------- |
+     274             : 
+     275             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     276             : 
+     277             :   OutputPublisher control_output_publisher_;
+     278             : 
+     279             :   ControlOutputModalities_t _hw_api_inputs_;
+     280             : 
+     281             :   double desired_uav_state_rate_ = 100.0;
+     282             : 
+     283             :   // this timer will check till we already got the hardware api diagnostics
+     284             :   // then it will trigger the initialization of the controllers and finish
+     285             :   // the initialization of the ControlManager
+     286             :   ros::Timer timer_hw_api_capabilities_;
+     287             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     288             : 
+     289             :   void preinitialize(void);
+     290             :   void initialize(void);
+     291             : 
+     292             :   // | ------------ tracker and controller switching ------------ |
+     293             : 
+     294             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     295             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     296             : 
+     297             :   // the time of last switching of a tracker or a controller
+     298             :   ros::Time  controller_tracker_switch_time_;
+     299             :   std::mutex mutex_controller_tracker_switch_time_;
+     300             : 
+     301             :   // | -------------------- the transformer  -------------------- |
+     302             : 
+     303             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     304             : 
+     305             :   // | ------------------- scope timer logger ------------------- |
+     306             : 
+     307             :   bool                                       scope_timer_enabled_ = false;
+     308             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     309             : 
+     310             :   // | --------------------- general params --------------------- |
+     311             : 
+     312             :   // defines the type of state input: odometry or uav_state mesasge types
+     313             :   int _state_input_;
+     314             : 
+     315             :   // names of important trackers
+     316             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     317             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     318             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     319             : 
+     320             :   // names of important controllers
+     321             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     322             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     323             : 
+     324             :   // joystick control
+     325             :   bool        _joystick_enabled_ = false;
+     326             :   int         _joystick_mode_;
+     327             :   std::string _joystick_tracker_name_;
+     328             :   std::string _joystick_controller_name_;
+     329             :   std::string _joystick_fallback_tracker_name_;
+     330             :   std::string _joystick_fallback_controller_name_;
+     331             : 
+     332             :   // should disarm after emergancy landing?
+     333             :   bool _eland_disarm_enabled_ = false;
+     334             : 
+     335             :   // enabling the emergency handoff -> will disable eland and failsafe
+     336             :   bool _rc_emergency_handoff_ = false;
+     337             : 
+     338             :   // what throttle should be output when null tracker is active?
+     339             :   double _min_throttle_null_tracker_ = 0.0;
+     340             : 
+     341             :   // rates of all the timers
+     342             :   double _status_timer_rate_   = 0;
+     343             :   double _safety_timer_rate_   = 0;
+     344             :   double _elanding_timer_rate_ = 0;
+     345             :   double _failsafe_timer_rate_ = 0;
+     346             :   double _bumper_timer_rate_   = 0;
+     347             : 
+     348             :   bool _snap_trajectory_to_safety_area_ = false;
+     349             : 
+     350             :   // | -------------- uav_state/odometry subscriber ------------- |
+     351             : 
+     352             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     353             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     354             : 
+     355             :   mrs_msgs::UavState uav_state_;
+     356             :   mrs_msgs::UavState previous_uav_state_;
+     357             :   bool               got_uav_state_               = false;
+     358             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     359             :   double             uav_roll_                    = 0;
+     360             :   double             uav_pitch_                   = 0;
+     361             :   double             uav_yaw_                     = 0;
+     362             :   double             uav_heading_                 = 0;
+     363             :   std::mutex         mutex_uav_state_;
+     364             : 
+     365             :   // odometry hiccup detection
+     366             :   double uav_state_avg_dt_        = 1;
+     367             :   double uav_state_hiccup_factor_ = 1;
+     368             :   int    uav_state_count_         = 0;
+     369             : 
+     370             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     371             : 
+     372             :   // | -------------- safety area max z subscriber -------------- |
+     373             : 
+     374             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     375             : 
+     376             :   // | ------------- odometry innovation subscriber ------------- |
+     377             : 
+     378             :   // odometry innovation is published by the odometry node
+     379             :   // it is used to issue eland if the estimator's input is too wonky
+     380             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     381             : 
+     382             :   // | --------------------- common handlers -------------------- |
+     383             : 
+     384             :   // contains handlers that are shared with trackers and controllers
+     385             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     386             : 
+     387             :   // | --------------- tracker and controller IDs --------------- |
+     388             : 
+     389             :   // keeping track of currently active controllers and trackers
+     390             :   unsigned int active_tracker_idx_    = 0;
+     391             :   unsigned int active_controller_idx_ = 0;
+     392             : 
+     393             :   // indeces of some notable trackers
+     394             :   unsigned int _ehover_tracker_idx_               = 0;
+     395             :   unsigned int _landoff_tracker_idx_              = 0;
+     396             :   unsigned int _joystick_tracker_idx_             = 0;
+     397             :   unsigned int _joystick_controller_idx_          = 0;
+     398             :   unsigned int _failsafe_controller_idx_          = 0;
+     399             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     400             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     401             :   unsigned int _null_tracker_idx_                 = 0;
+     402             :   unsigned int _eland_controller_idx_             = 0;
+     403             : 
+     404             :   // | -------------- enabling the output publisher ------------- |
+     405             : 
+     406             :   void              toggleOutput(const bool& input);
+     407             :   std::atomic<bool> output_enabled_ = false;
+     408             : 
+     409             :   // | ----------------------- publishers ----------------------- |
+     410             : 
+     411             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     413             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     414             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     415             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     416             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     417             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     418             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     419             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     420             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     421             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     422             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     423             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     424             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     425             :   mrs_lib::PublisherHandler<mrs_msgs::BumperStatus>              ph_bumper_status_;
+     426             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     427             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     428             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     429             : 
+     430             :   // | --------------------- service servers -------------------- |
+     431             : 
+     432             :   ros::ServiceServer service_server_switch_tracker_;
+     433             :   ros::ServiceServer service_server_switch_controller_;
+     434             :   ros::ServiceServer service_server_reset_tracker_;
+     435             :   ros::ServiceServer service_server_hover_;
+     436             :   ros::ServiceServer service_server_ehover_;
+     437             :   ros::ServiceServer service_server_failsafe_;
+     438             :   ros::ServiceServer service_server_failsafe_escalating_;
+     439             :   ros::ServiceServer service_server_toggle_output_;
+     440             :   ros::ServiceServer service_server_arm_;
+     441             :   ros::ServiceServer service_server_enable_callbacks_;
+     442             :   ros::ServiceServer service_server_set_constraints_;
+     443             :   ros::ServiceServer service_server_use_joystick_;
+     444             :   ros::ServiceServer service_server_use_safety_area_;
+     445             :   ros::ServiceServer service_server_emergency_reference_;
+     446             :   ros::ServiceServer service_server_pirouette_;
+     447             :   ros::ServiceServer service_server_eland_;
+     448             :   ros::ServiceServer service_server_parachute_;
+     449             : 
+     450             :   // human callbable services for references
+     451             :   ros::ServiceServer service_server_goto_;
+     452             :   ros::ServiceServer service_server_goto_fcu_;
+     453             :   ros::ServiceServer service_server_goto_relative_;
+     454             :   ros::ServiceServer service_server_goto_altitude_;
+     455             :   ros::ServiceServer service_server_set_heading_;
+     456             :   ros::ServiceServer service_server_set_heading_relative_;
+     457             : 
+     458             :   // the reference service and subscriber
+     459             :   ros::ServiceServer                                    service_server_reference_;
+     460             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     461             : 
+     462             :   // the velocity reference service and subscriber
+     463             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     464             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     465             : 
+     466             :   // trajectory tracking
+     467             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     468             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     469             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     470             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     471             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     472             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     473             : 
+     474             :   // transform service servers
+     475             :   ros::ServiceServer service_server_transform_reference_;
+     476             :   ros::ServiceServer service_server_transform_pose_;
+     477             :   ros::ServiceServer service_server_transform_vector3_;
+     478             : 
+     479             :   // safety area services
+     480             :   ros::ServiceServer service_server_validate_reference_;
+     481             :   ros::ServiceServer service_server_validate_reference_2d_;
+     482             :   ros::ServiceServer service_server_validate_reference_list_;
+     483             : 
+     484             :   // bumper service servers
+     485             :   ros::ServiceServer service_server_bumper_enabler_;
+     486             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     487             : 
+     488             :   // service clients
+     489             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     490             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     491             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     492             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     493             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     494             : 
+     495             :   // safety area min z servers
+     496             :   ros::ServiceServer service_server_set_min_z_;
+     497             :   ros::ServiceServer service_server_get_min_z_;
+     498             : 
+     499             :   // | --------- trackers' and controllers' last results -------- |
+     500             : 
+     501             :   // the last result of an active tracker
+     502             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     503             :   std::mutex                              mutex_last_tracker_cmd_;
+     504             : 
+     505             :   // the last result of an active controller
+     506             :   Controller::ControlOutput last_control_output_;
+     507             :   std::mutex                mutex_last_control_output_;
+     508             : 
+     509             :   // | -------------- HW API diagnostics subscriber ------------- |
+     510             : 
+     511             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     512             : 
+     513             :   bool offboard_mode_          = false;
+     514             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     515             :   bool armed_                  = false;
+     516             : 
+     517             :   // | -------------------- throttle and mass ------------------- |
+     518             : 
+     519             :   // throttle mass estimation during eland
+     520             :   double    throttle_mass_estimate_   = 0;
+     521             :   bool      throttle_under_threshold_ = false;
+     522             :   ros::Time throttle_mass_estimate_first_time_;
+     523             : 
+     524             :   // | ---------------------- safety params --------------------- |
+     525             : 
+     526             :   // failsafe when tilt error is too large
+     527             :   bool   _tilt_error_disarm_enabled_;
+     528             :   double _tilt_error_disarm_timeout_;
+     529             :   double _tilt_error_disarm_threshold_;
+     530             : 
+     531             :   ros::Time tilt_error_disarm_time_;
+     532             :   bool      tilt_error_disarm_over_thr_ = false;
+     533             : 
+     534             :   // elanding when tilt error is too large
+     535             :   bool   _tilt_limit_eland_enabled_;
+     536             :   double _tilt_limit_eland_ = 0;  // [rad]
+     537             : 
+     538             :   // disarming when tilt error is too large
+     539             :   bool   _tilt_limit_disarm_enabled_;
+     540             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     541             : 
+     542             :   // elanding when yaw error is too large
+     543             :   bool   _yaw_error_eland_enabled_;
+     544             :   double _yaw_error_eland_ = 0;  // [rad]
+     545             : 
+     546             :   // keeping track of control errors
+     547             :   std::optional<double> tilt_error_ = 0;
+     548             :   std::optional<double> yaw_error_  = 0;
+     549             :   std::mutex            mutex_attitude_error_;
+     550             : 
+     551             :   std::optional<Eigen::Vector3d> position_error_;
+     552             :   std::mutex                     mutex_position_error_;
+     553             : 
+     554             :   // control error for triggering failsafe, eland, etc.
+     555             :   // this filled with the current controllers failsafe threshold
+     556             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     557             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     558             :   bool   _odometry_innovation_check_enabled_ = false;
+     559             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     560             : 
+     561             :   bool callbacks_enabled_ = true;
+     562             : 
+     563             :   // | ------------------------ parachute ----------------------- |
+     564             : 
+     565             :   bool _parachute_enabled_ = false;
+     566             : 
+     567             :   std::tuple<bool, std::string> deployParachute(void);
+     568             :   bool                          parachuteSrv(void);
+     569             : 
+     570             :   // | ----------------------- safety area ---------------------- |
+     571             : 
+     572             :   // safety area
+     573             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     574             : 
+     575             :   std::atomic<bool> use_safety_area_ = false;
+     576             : 
+     577             :   std::string _safety_area_horizontal_frame_;
+     578             :   std::string _safety_area_vertical_frame_;
+     579             : 
+     580             :   double _safety_area_min_z_ = 0;
+     581             :   double _safety_area_max_z_ = 0;
+     582             : 
+     583             :   // safety area routines
+     584             :   // those are passed to trackers using the common_handlers object
+     585             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     586             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     587             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     588             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     589             :   double getMinZ(const std::string& frame_id);
+     590             :   double getMaxZ(const std::string& frame_id);
+     591             : 
+     592             :   // | ------------------------ callbacks ----------------------- |
+     593             : 
+     594             :   // topic callbacks
+     595             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     596             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     597             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     598             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     599             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     600             : 
+     601             :   // topic timeouts
+     602             :   void timeoutUavState(const double& missing_for);
+     603             : 
+     604             :   // switching controller and tracker services
+     605             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     606             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     607             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     608             : 
+     609             :   // reference callbacks
+     610             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     611             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     612             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     613             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     614             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     615             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     616             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     617             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     618             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     619             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     620             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     621             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     622             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     623             : 
+     624             :   // safety callbacks
+     625             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     626             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     627             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     628             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     634             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     635             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     636             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     637             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     638             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     639             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     640             :   bool callbackBumperSetParams(mrs_msgs::BumperParamsSrv::Request& req, mrs_msgs::BumperParamsSrv::Response& res);
+     641             : 
+     642             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     643             : 
+     644             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     645             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     646             :   bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+     647             : 
+     648             :   // transformation callbacks
+     649             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     650             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     651             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     652             : 
+     653             :   // | ----------------------- constraints ---------------------- |
+     654             : 
+     655             :   // sets constraints to all trackers
+     656             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     657             : 
+     658             :   // constraints management
+     659             :   bool              got_constraints_ = false;
+     660             :   std::mutex        mutex_constraints_;
+     661             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     662             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     663             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     664             :   std::atomic<bool> constraints_being_enforced_ = false;
+     665             : 
+     666             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     667             : 
+     668             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     669             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     670             : 
+     671             :   // | ------------------ emergency triggered? ------------------ |
+     672             : 
+     673             :   bool failsafe_triggered_ = false;
+     674             :   bool eland_triggered_    = false;
+     675             : 
+     676             :   // | ------------------------- timers ------------------------- |
+     677             : 
+     678             :   // timer for regular status publishing
+     679             :   ros::Timer timer_status_;
+     680             :   void       timerStatus(const ros::TimerEvent& event);
+     681             : 
+     682             :   // timer for issuing the failsafe landing
+     683             :   ros::Timer timer_failsafe_;
+     684             :   void       timerFailsafe(const ros::TimerEvent& event);
+     685             : 
+     686             :   // oneshot timer for running controllers and trackers
+     687             :   void              asyncControl(void);
+     688             :   std::atomic<bool> running_async_control_ = false;
+     689             :   std::future<void> async_control_result_;
+     690             : 
+     691             :   // timer for issuing emergancy landing
+     692             :   ros::Timer timer_eland_;
+     693             :   void       timerEland(const ros::TimerEvent& event);
+     694             : 
+     695             :   // timer for regular checking of controller errors
+     696             :   ros::Timer        timer_safety_;
+     697             :   void              timerSafety(const ros::TimerEvent& event);
+     698             :   std::atomic<bool> running_safety_timer_        = false;
+     699             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     700             : 
+     701             :   // timer for issuing the pirouette
+     702             :   ros::Timer timer_pirouette_;
+     703             :   void       timerPirouette(const ros::TimerEvent& event);
+     704             : 
+     705             :   // | --------------------- obstacle bumper -------------------- |
+     706             : 
+     707             :   // bumper timer
+     708             :   ros::Timer timer_bumper_;
+     709             :   void       timerBumper(const ros::TimerEvent& event);
+     710             : 
+     711             :   // bumper subscriber
+     712             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     713             : 
+     714             :   bool        _bumper_switch_tracker_    = false;
+     715             :   bool        _bumper_switch_controller_ = false;
+     716             :   std::string _bumper_tracker_name_;
+     717             :   std::string _bumper_controller_name_;
+     718             :   std::string bumper_previous_tracker_;
+     719             :   std::string bumper_previous_controller_;
+     720             : 
+     721             :   std::atomic<bool> bumper_enabled_ = false;
+     722             :   std::atomic<bool> repulsing_      = false;
+     723             : 
+     724             :   double _bumper_horizontal_distance_ = 0;
+     725             :   double _bumper_vertical_distance_   = 0;
+     726             : 
+     727             :   double _bumper_horizontal_overshoot_ = 0;
+     728             :   double _bumper_vertical_overshoot_   = 0;
+     729             : 
+     730             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     731             :   bool bumperPushFromObstacle(void);
+     732             : 
+     733             :   // | --------------- safety checks and failsafes -------------- |
+     734             : 
+     735             :   // escalating failsafe (eland -> failsafe -> disarm)
+     736             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     737             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     738             :   double                     _escalating_failsafe_timeout_         = 0;
+     739             :   ros::Time                  escalating_failsafe_time_;
+     740             :   bool                       _escalating_failsafe_ehover_   = false;
+     741             :   bool                       _escalating_failsafe_eland_    = false;
+     742             :   bool                       _escalating_failsafe_failsafe_ = false;
+     743             :   double                     _rc_escalating_failsafe_threshold_;
+     744             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     745             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     746             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     747             : 
+     748             :   std::string _tracker_error_action_;
+     749             : 
+     750             :   // emergancy landing state machine
+     751             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     752             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     753             :   std::mutex      mutex_landing_state_machine_;
+     754             :   void            changeLandingState(LandingStates_t new_state);
+     755             :   double          _uav_mass_ = 0;
+     756             :   double          _elanding_cutoff_mass_factor_;
+     757             :   double          _elanding_cutoff_timeout_;
+     758             :   double          landing_uav_mass_ = 0;
+     759             : 
+     760             :   // initial body disturbance loaded from params
+     761             :   double _initial_body_disturbance_x_ = 0;
+     762             :   double _initial_body_disturbance_y_ = 0;
+     763             : 
+     764             :   // profiling
+     765             :   mrs_lib::Profiler profiler_;
+     766             :   bool              _profiler_enabled_ = false;
+     767             : 
+     768             :   // automatic pc shutdown (DARPA specific)
+     769             :   bool _automatic_pc_shutdown_enabled_ = false;
+     770             : 
+     771             :   // diagnostics publishing
+     772             :   void       publishDiagnostics(void);
+     773             :   std::mutex mutex_diagnostics_;
+     774             : 
+     775             :   void                                             ungripSrv(void);
+     776             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     777             : 
+     778             :   bool isFlyingNormally(void);
+     779             : 
+     780             :   // | ------------------------ pirouette ----------------------- |
+     781             : 
+     782             :   bool       _pirouette_enabled_ = false;
+     783             :   double     _pirouette_speed_;
+     784             :   double     _pirouette_timer_rate_;
+     785             :   std::mutex mutex_pirouette_;
+     786             :   double     pirouette_initial_heading_;
+     787             :   double     pirouette_iterator_;
+     788             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     789             : 
+     790             :   // | -------------------- joystick control -------------------- |
+     791             : 
+     792             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     793             : 
+     794             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     795             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     796             : 
+     797             :   // joystick buttons mappings
+     798             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     799             : 
+     800             :   // channel numbers and channel multipliers
+     801             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     802             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     803             : 
+     804             :   ros::Timer timer_joystick_;
+     805             :   void       timerJoystick(const ros::TimerEvent& event);
+     806             :   double     _joystick_timer_rate_ = 0;
+     807             : 
+     808             :   double _joystick_carrot_distance_ = 0;
+     809             : 
+     810             :   ros::Time joystick_start_press_time_;
+     811             :   bool      joystick_start_pressed_ = false;
+     812             : 
+     813             :   ros::Time joystick_back_press_time_;
+     814             :   bool      joystick_back_pressed_ = false;
+     815             :   bool      joystick_goto_enabled_ = false;
+     816             : 
+     817             :   bool      joystick_failsafe_pressed_ = false;
+     818             :   ros::Time joystick_failsafe_press_time_;
+     819             : 
+     820             :   bool      joystick_eland_pressed_ = false;
+     821             :   ros::Time joystick_eland_press_time_;
+     822             : 
+     823             :   // | ------------------- RC joystick control ------------------ |
+     824             : 
+     825             :   // listening to the RC channels as told by pixhawk
+     826             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     827             : 
+     828             :   // the RC channel mapping of the main 4 control signals
+     829             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     830             : 
+     831             :   bool              _rc_goto_enabled_               = false;
+     832             :   std::atomic<bool> rc_goto_active_                 = false;
+     833             :   double            rc_joystick_channel_last_value_ = 0.5;
+     834             :   bool              rc_joystick_channel_was_low_    = false;
+     835             :   int               _rc_joystick_channel_           = 0;
+     836             : 
+     837             :   double _rc_horizontal_speed_ = 0;
+     838             :   double _rc_vertical_speed_   = 0;
+     839             :   double _rc_heading_rate_     = 0;
+     840             : 
+     841             :   // | ------------------- trajectory loading ------------------- |
+     842             : 
+     843             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     844             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     845             : 
+     846             :   // | --------------------- other routines --------------------- |
+     847             : 
+     848             :   // this is called to update the trackers and to receive position control command from the active one
+     849             :   void updateTrackers(void);
+     850             : 
+     851             :   // this is called to update the controllers and to receive attitude control command from the active one
+     852             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     853             : 
+     854             :   // sets the reference to the active tracker
+     855             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     856             : 
+     857             :   // sets the velocity reference to the active tracker
+     858             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     859             : 
+     860             :   // sets the reference trajectory to the active tracker
+     861             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     862             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     863             : 
+     864             :   // publishes
+     865             :   void publish(void);
+     866             : 
+     867             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     868             : 
+     869             :   double getMass(void);
+     870             : 
+     871             :   // publishes rviz-visualizable control reference
+     872             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     873             : 
+     874             :   void initializeControlOutput(void);
+     875             : 
+     876             :   // tell the mrs_odometry to disable its callbacks
+     877             :   void odometryCallbacksSrv(const bool input);
+     878             : 
+     879             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     880             : 
+     881             :   void                          shutdown();
+     882             :   void                          setCallbacks(bool in);
+     883             :   bool                          isOffboard(void);
+     884             :   bool                          elandSrv(void);
+     885             :   std::tuple<bool, std::string> arming(const bool input);
+     886             : 
+     887             :   // safety functions impl
+     888             :   std::tuple<bool, std::string> ehover(void);
+     889             :   std::tuple<bool, std::string> hover(void);
+     890             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     891             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     892             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     893             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     894             :   std::tuple<bool, std::string> eland(void);
+     895             :   std::tuple<bool, std::string> failsafe(void);
+     896             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     897             : 
+     898             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     899             : };
+     900             : 
+     901             : //}
+     902             : 
+     903             : /* //{ onInit() */
+     904             : 
+     905          42 : void ControlManager::onInit() {
+     906          42 :   preinitialize();
+     907          42 : }
+     908             : 
+     909             : //}
+     910             : 
+     911             : /* preinitialize() //{ */
+     912             : 
+     913          42 : void ControlManager::preinitialize(void) {
+     914             : 
+     915          42 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     916             : 
+     917          42 :   ros::Time::waitForValid();
+     918             : 
+     919          42 :   mrs_lib::SubscribeHandlerOptions shopts;
+     920          42 :   shopts.nh                 = nh_;
+     921          42 :   shopts.node_name          = "ControlManager";
+     922          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     923          42 :   shopts.threadsafe         = true;
+     924          42 :   shopts.autostart          = true;
+     925          42 :   shopts.queue_size         = 10;
+     926          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     927             : 
+     928          42 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     929             : 
+     930          42 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     931          42 : }
+     932             : 
+     933             : //}
+     934             : 
+     935             : /* initialize() //{ */
+     936             : 
+     937          42 : void ControlManager::initialize(void) {
+     938             : 
+     939          42 :   joystick_start_press_time_      = ros::Time(0);
+     940          42 :   joystick_failsafe_press_time_   = ros::Time(0);
+     941          42 :   joystick_eland_press_time_      = ros::Time(0);
+     942          42 :   escalating_failsafe_time_       = ros::Time(0);
+     943          42 :   controller_tracker_switch_time_ = ros::Time(0);
+     944             : 
+     945          42 :   ROS_INFO("[ControlManager]: initializing");
+     946             : 
+     947             :   // --------------------------------------------------------------
+     948             :   // |         common handler for trackers and controllers        |
+     949             :   // --------------------------------------------------------------
+     950             : 
+     951          42 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     952             : 
+     953             :   // --------------------------------------------------------------
+     954             :   // |                           params                           |
+     955             :   // --------------------------------------------------------------
+     956             : 
+     957          84 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     958             : 
+     959          42 :   param_loader.loadParam("custom_config", _custom_config_);
+     960          42 :   param_loader.loadParam("platform_config", _platform_config_);
+     961          42 :   param_loader.loadParam("world_config", _world_config_);
+     962          42 :   param_loader.loadParam("network_config", _network_config_);
+     963             : 
+     964          42 :   if (_custom_config_ != "") {
+     965          42 :     param_loader.addYamlFile(_custom_config_);
+     966             :   }
+     967             : 
+     968          42 :   if (_platform_config_ != "") {
+     969          42 :     param_loader.addYamlFile(_platform_config_);
+     970             :   }
+     971             : 
+     972          42 :   if (_world_config_ != "") {
+     973          42 :     param_loader.addYamlFile(_world_config_);
+     974             :   }
+     975             : 
+     976          42 :   if (_network_config_ != "") {
+     977          42 :     param_loader.addYamlFile(_network_config_);
+     978             :   }
+     979             : 
+     980          42 :   param_loader.addYamlFileFromParam("private_config");
+     981          42 :   param_loader.addYamlFileFromParam("public_config");
+     982          42 :   param_loader.addYamlFileFromParam("private_trackers");
+     983          42 :   param_loader.addYamlFileFromParam("private_controllers");
+     984          42 :   param_loader.addYamlFileFromParam("public_controllers");
+     985             : 
+     986             :   // params passed from the launch file are not prefixed
+     987          42 :   param_loader.loadParam("uav_name", _uav_name_);
+     988          42 :   param_loader.loadParam("body_frame", _body_frame_);
+     989          42 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     990          42 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     991          42 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     992          42 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     993          42 :   param_loader.loadParam("g", common_handlers_->g);
+     994             : 
+     995             :   // motor params are also not prefixed, since they are common to more nodes
+     996          42 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+     997          42 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+     998          42 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+     999             : 
+    1000             :   // | ----------------------- safety area ---------------------- |
+    1001             : 
+    1002             :   bool use_safety_area;
+    1003          42 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1004          42 :   use_safety_area_ = use_safety_area;
+    1005             : 
+    1006          42 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1007             : 
+    1008          42 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1009          42 :   param_loader.loadParam("safety_area/vertical/min_z", _safety_area_min_z_);
+    1010          42 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1011             : 
+    1012          42 :   if (use_safety_area_) {
+    1013             : 
+    1014         117 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1015             : 
+    1016             :     try {
+    1017             : 
+    1018          78 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1019          39 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1020             : 
+    1021          39 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1022             :     }
+    1023             : 
+    1024           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1025           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1026           0 :       ros::shutdown();
+    1027             :     }
+    1028           0 :     catch (...) {
+    1029           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1030           0 :       ros::shutdown();
+    1031             :     }
+    1032             : 
+    1033          39 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1034             :   }
+    1035             : 
+    1036          42 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1037             : 
+    1038          42 :   param_loader.loadParam("state_input", _state_input_);
+    1039             : 
+    1040          42 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1041           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1042           0 :     ros::shutdown();
+    1043             :   }
+    1044             : 
+    1045          42 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1046          42 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1047          42 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1048             : 
+    1049          42 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1050          42 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1051          42 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1052          42 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1053          42 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1054             : 
+    1055          42 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1056          42 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1057          42 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1058          42 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1059          42 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1060          42 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1061          42 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1062          42 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1063             : 
+    1064          42 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1065          42 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1066             : 
+    1067          42 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1068             : 
+    1069          42 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1070           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1071           0 :     ros::shutdown();
+    1072             :   }
+    1073             : 
+    1074          42 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1075          42 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1076             : 
+    1077          42 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1078             : 
+    1079          42 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1080           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1081           0 :     ros::shutdown();
+    1082             :   }
+    1083             : 
+    1084          42 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1085          42 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1086             : 
+    1087          42 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1088             : 
+    1089          42 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1090           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1091           0 :     ros::shutdown();
+    1092             :   }
+    1093             : 
+    1094          42 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1095          42 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1096          42 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1097          42 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1098             : 
+    1099          42 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1100          42 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1101             : 
+    1102          42 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1103          42 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1104          42 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1105             : 
+    1106          42 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1107             : 
+    1108          42 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1109           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1110           0 :     ros::shutdown();
+    1111             :   }
+    1112             : 
+    1113             :   // default constraints
+    1114             : 
+    1115          42 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1116          42 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1117          42 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1118          42 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1119             : 
+    1120          42 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1121          42 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1122          42 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1123          42 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1124             : 
+    1125          42 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1126          42 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1127          42 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1128          42 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1129             : 
+    1130          42 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1131          42 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1132          42 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1133          42 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1134             : 
+    1135          42 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1136          42 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1137          42 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1138             : 
+    1139          42 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1140             : 
+    1141          42 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1142             : 
+    1143             :   // joystick
+    1144             : 
+    1145          42 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1146          42 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1147          42 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1148          42 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1149          42 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1150          42 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1151          42 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1152          42 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1153             : 
+    1154          42 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1155          42 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1156          42 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1157          42 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1158          42 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1159          42 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1160          42 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1161          42 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1162          42 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1163          42 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1164             : 
+    1165             :   // load channels
+    1166          42 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1167          42 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1168          42 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1169          42 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1170             : 
+    1171             :   // load channel multipliers
+    1172          42 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1173          42 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1174          42 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1175          42 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1176             : 
+    1177             :   bool bumper_enabled;
+    1178          42 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1179          42 :   bumper_enabled_ = bumper_enabled;
+    1180             : 
+    1181          42 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1182          42 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1183          42 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1184          42 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1185          42 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1186             : 
+    1187          42 :   param_loader.loadParam("obstacle_bumper/horizontal/threshold_distance", _bumper_horizontal_distance_);
+    1188          42 :   param_loader.loadParam("obstacle_bumper/vertical/threshold_distance", _bumper_vertical_distance_);
+    1189             : 
+    1190          42 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1191          42 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1192             : 
+    1193          42 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1194             : 
+    1195          42 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1196             : 
+    1197             :   // check the values of tracker error action
+    1198          42 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1199           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1200             :               EHOVER_STR);
+    1201           0 :     ros::shutdown();
+    1202             :   }
+    1203             : 
+    1204          42 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1205          42 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1206          42 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1207          42 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1208          42 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1209             : 
+    1210          42 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1211          42 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1212          42 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1213          42 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1214             : 
+    1215          42 :   param_loader.loadParam("automatic_pc_shutdown/enabled", _automatic_pc_shutdown_enabled_);
+    1216             : 
+    1217          42 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1218          42 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1219             : 
+    1220          42 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1221             : 
+    1222             :   // --------------------------------------------------------------
+    1223             :   // |             initialize the last control output             |
+    1224             :   // --------------------------------------------------------------
+    1225             : 
+    1226          42 :   initializeControlOutput();
+    1227             : 
+    1228             :   // | --------------------- tf transformer --------------------- |
+    1229             : 
+    1230          42 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1231          42 :   transformer_->setDefaultPrefix(_uav_name_);
+    1232          42 :   transformer_->retryLookupNewest(true);
+    1233             : 
+    1234             :   // | ------------------- scope timer logger ------------------- |
+    1235             : 
+    1236          42 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1237         126 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1238          42 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1239             : 
+    1240             :   // bind transformer to trackers and controllers for use
+    1241          42 :   common_handlers_->transformer = transformer_;
+    1242             : 
+    1243             :   // bind scope timer to trackers and controllers for use
+    1244          42 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1245          42 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1246             : 
+    1247          42 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1248          42 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1249          42 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1250          42 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1251          42 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1252             : 
+    1253          42 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1254             : 
+    1255          42 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1256             : 
+    1257          42 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1258             : 
+    1259          42 :   common_handlers_->uav_name = _uav_name_;
+    1260             : 
+    1261          42 :   common_handlers_->parent_nh = nh_;
+    1262             : 
+    1263             :   // --------------------------------------------------------------
+    1264             :   // |                        load trackers                       |
+    1265             :   // --------------------------------------------------------------
+    1266             : 
+    1267          84 :   std::vector<std::string> custom_trackers;
+    1268             : 
+    1269          42 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1270          42 :   param_loader.loadParam("trackers", custom_trackers);
+    1271             : 
+    1272          42 :   if (!custom_trackers.empty()) {
+    1273           1 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1274             :   }
+    1275             : 
+    1276          42 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1277          42 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1278             : 
+    1279          42 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1280             : 
+    1281         295 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1282             : 
+    1283         506 :     std::string tracker_name = _tracker_names_[i];
+    1284             : 
+    1285             :     // load the controller parameters
+    1286         506 :     std::string address;
+    1287         506 :     std::string name_space;
+    1288             :     bool        human_switchable;
+    1289             : 
+    1290         253 :     param_loader.loadParam(tracker_name + "/address", address);
+    1291         253 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1292         253 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1293             : 
+    1294         759 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1295         253 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1296             : 
+    1297             :     try {
+    1298         253 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1299         253 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1300             :     }
+    1301           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1302           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1303           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1304           0 :       ros::shutdown();
+    1305             :     }
+    1306           0 :     catch (pluginlib::PluginlibException& ex) {
+    1307           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1308           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1309           0 :       ros::shutdown();
+    1310             :     }
+    1311             :   }
+    1312             : 
+    1313          42 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1314             : 
+    1315         295 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1316             : 
+    1317         253 :     std::map<std::string, TrackerParams>::iterator it;
+    1318         253 :     it = trackers_.find(_tracker_names_[i]);
+    1319             : 
+    1320             :     // create private handlers
+    1321             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1322         506 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1323             : 
+    1324         253 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1325         253 :     private_handlers->name_space     = it->second.name_space;
+    1326         253 :     private_handlers->runtime_name   = _tracker_names_[i];
+    1327         253 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_[i]);
+    1328             : 
+    1329         253 :     if (_custom_config_ != "") {
+    1330         253 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1331             :     }
+    1332             : 
+    1333         253 :     if (_platform_config_ != "") {
+    1334         253 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1335             :     }
+    1336             : 
+    1337         253 :     if (_world_config_ != "") {
+    1338         253 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1339             :     }
+    1340             : 
+    1341         253 :     if (_network_config_ != "") {
+    1342         253 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1343             :     }
+    1344             : 
+    1345         253 :     bool success = false;
+    1346             : 
+    1347             :     try {
+    1348         253 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1349         253 :       success = tracker_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1350             :     }
+    1351           0 :     catch (std::runtime_error& ex) {
+    1352           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1353             :     }
+    1354             : 
+    1355         253 :     if (!success) {
+    1356           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1357           0 :       ros::shutdown();
+    1358             :     }
+    1359             :   }
+    1360             : 
+    1361          42 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1362             : 
+    1363             :   // --------------------------------------------------------------
+    1364             :   // |           check the existance of selected trackers         |
+    1365             :   // --------------------------------------------------------------
+    1366             : 
+    1367             :   // | ------ check for the existance of the hover tracker ------ |
+    1368             : 
+    1369             :   // check if the hover_tracker is within the loaded trackers
+    1370             :   {
+    1371          42 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1372             : 
+    1373          42 :     if (idx) {
+    1374          42 :       _ehover_tracker_idx_ = idx.value();
+    1375             :     } else {
+    1376           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1377           0 :       ros::shutdown();
+    1378             :     }
+    1379             :   }
+    1380             : 
+    1381             :   // | ----- check for the existence of the landoff tracker ----- |
+    1382             : 
+    1383             :   {
+    1384          42 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1385             : 
+    1386          42 :     if (idx) {
+    1387          42 :       _landoff_tracker_idx_ = idx.value();
+    1388             :     } else {
+    1389           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1390           0 :       ros::shutdown();
+    1391             :     }
+    1392             :   }
+    1393             : 
+    1394             :   // | ------- check for the existence of the null tracker ------ |
+    1395             : 
+    1396             :   {
+    1397          42 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1398             : 
+    1399          42 :     if (idx) {
+    1400          42 :       _null_tracker_idx_ = idx.value();
+    1401             :     } else {
+    1402           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1403           0 :       ros::shutdown();
+    1404             :     }
+    1405             :   }
+    1406             : 
+    1407             :   // --------------------------------------------------------------
+    1408             :   // |         check existance of trackers for joystick           |
+    1409             :   // --------------------------------------------------------------
+    1410             : 
+    1411          42 :   if (_joystick_enabled_) {
+    1412             : 
+    1413          42 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1414             : 
+    1415          42 :     if (idx) {
+    1416          42 :       _joystick_tracker_idx_ = idx.value();
+    1417             :     } else {
+    1418           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1419           0 :       ros::shutdown();
+    1420             :     }
+    1421             :   }
+    1422             : 
+    1423          42 :   if (_bumper_switch_tracker_) {
+    1424             : 
+    1425          42 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1426             : 
+    1427          42 :     if (!idx) {
+    1428           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1429           0 :       ros::shutdown();
+    1430             :     }
+    1431             :   }
+    1432             : 
+    1433             :   {
+    1434          42 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1435             : 
+    1436          42 :     if (idx) {
+    1437          42 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1438             :     } else {
+    1439           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1440           0 :       ros::shutdown();
+    1441             :     }
+    1442             :   }
+    1443             : 
+    1444             :   // --------------------------------------------------------------
+    1445             :   // |                    load the controllers                    |
+    1446             :   // --------------------------------------------------------------
+    1447             : 
+    1448          84 :   std::vector<std::string> custom_controllers;
+    1449             : 
+    1450          42 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1451          42 :   param_loader.loadParam("controllers", custom_controllers);
+    1452             : 
+    1453          42 :   if (!custom_controllers.empty()) {
+    1454           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1455             :   }
+    1456             : 
+    1457          42 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1458             : 
+    1459             :   // for each controller in the list
+    1460         252 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1461             : 
+    1462         420 :     std::string controller_name = _controller_names_[i];
+    1463             : 
+    1464             :     // load the controller parameters
+    1465         420 :     std::string address;
+    1466         420 :     std::string name_space;
+    1467             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1468             :     bool        human_switchable;
+    1469         210 :     param_loader.loadParam(controller_name + "/address", address);
+    1470         210 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1471         210 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1472         210 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1473         210 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1474         210 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1475             : 
+    1476             :     // check if the controller can output some of the required outputs
+    1477             :     {
+    1478             : 
+    1479         210 :       ControlOutputModalities_t outputs;
+    1480         210 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1481         210 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1482         210 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1483         210 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1484         210 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1485         210 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1486         210 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1487         210 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1488         210 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1489             : 
+    1490         210 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1491         210 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1492         210 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1493         210 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1494         210 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1495         210 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1496         210 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1497         210 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1498         210 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1499             : 
+    1500         200 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1501         410 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1502             : 
+    1503         210 :       if (!meets_requirements) {
+    1504             : 
+    1505           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1506             :                   controller_name.c_str());
+    1507             : 
+    1508           0 :         if (_hw_api_inputs_.actuators) {
+    1509           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1510             :         }
+    1511             : 
+    1512           0 :         if (_hw_api_inputs_.control_group) {
+    1513           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1514             :         }
+    1515             : 
+    1516           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1517           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1518             :         }
+    1519             : 
+    1520           0 :         if (_hw_api_inputs_.attitude) {
+    1521           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1522             :         }
+    1523             : 
+    1524           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1525           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1526             :         }
+    1527             : 
+    1528           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1529           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1530             :         }
+    1531             : 
+    1532           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1533           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1534             :         }
+    1535             : 
+    1536           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1537           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1538             :         }
+    1539             : 
+    1540           0 :         if (_hw_api_inputs_.position) {
+    1541           0 :           ROS_ERROR("[ControlManager]: - position");
+    1542             :         }
+    1543             : 
+    1544           0 :         ros::shutdown();
+    1545             :       }
+    1546             : 
+    1547         210 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1548           0 :         ROS_ERROR(
+    1549             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1550           0 :         ros::shutdown();
+    1551             :       }
+    1552             :     }
+    1553             : 
+    1554             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1555             : 
+    1556         210 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1557             : 
+    1558         210 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1559          20 :       _safety_timer_rate_     = 200.0;
+    1560          20 :       desired_uav_state_rate_ = 250.0;
+    1561         190 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1562         140 :       _safety_timer_rate_     = 100.0;
+    1563         140 :       desired_uav_state_rate_ = 100.0;
+    1564          50 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1565          20 :       _safety_timer_rate_     = 30.0;
+    1566          20 :       _status_timer_rate_     = 1.0;
+    1567          20 :       desired_uav_state_rate_ = 40.0;
+    1568             : 
+    1569          20 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1570           4 :         _uav_state_max_missing_time_ = 0.2;
+    1571             :       }
+    1572          30 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1573          30 :       _safety_timer_rate_     = 20.0;
+    1574          30 :       _status_timer_rate_     = 1.0;
+    1575          30 :       desired_uav_state_rate_ = 20.0;
+    1576             : 
+    1577          30 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1578           6 :         _uav_state_max_missing_time_ = 1.0;
+    1579             :       }
+    1580             :     }
+    1581             : 
+    1582         210 :     if (eland_threshold == 0) {
+    1583          43 :       eland_threshold = 1e6;
+    1584             :     }
+    1585             : 
+    1586         210 :     if (failsafe_threshold == 0) {
+    1587          43 :       failsafe_threshold = 1e6;
+    1588             :     }
+    1589             : 
+    1590         210 :     if (odometry_innovation_threshold == 0) {
+    1591          44 :       odometry_innovation_threshold = 1e6;
+    1592             :     }
+    1593             : 
+    1594         630 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1595         210 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1596             : 
+    1597             :     try {
+    1598         210 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1599         210 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1600             :     }
+    1601           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1602           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1603           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1604           0 :       ros::shutdown();
+    1605             :     }
+    1606           0 :     catch (pluginlib::PluginlibException& ex) {
+    1607           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1608           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1609           0 :       ros::shutdown();
+    1610             :     }
+    1611             :   }
+    1612             : 
+    1613          42 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1614             : 
+    1615         252 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1616             : 
+    1617         210 :     std::map<std::string, ControllerParams>::iterator it;
+    1618         210 :     it = controllers_.find(_controller_names_[i]);
+    1619             : 
+    1620             :     // create private handlers
+    1621             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1622         420 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1623             : 
+    1624         210 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1625         210 :     private_handlers->name_space     = it->second.name_space;
+    1626         210 :     private_handlers->runtime_name   = _controller_names_[i];
+    1627         210 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_[i]);
+    1628             : 
+    1629         210 :     if (_custom_config_ != "") {
+    1630         210 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1631             :     }
+    1632             : 
+    1633         210 :     if (_platform_config_ != "") {
+    1634         210 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1635             :     }
+    1636             : 
+    1637         210 :     if (_world_config_ != "") {
+    1638         210 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1639             :     }
+    1640             : 
+    1641         210 :     if (_network_config_ != "") {
+    1642         210 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1643             :     }
+    1644             : 
+    1645         210 :     bool success = false;
+    1646             : 
+    1647             :     try {
+    1648             : 
+    1649         210 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1650         210 :       success = controller_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1651             :     }
+    1652           0 :     catch (std::runtime_error& ex) {
+    1653           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1654             :     }
+    1655             : 
+    1656         210 :     if (!success) {
+    1657           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1658           0 :       ros::shutdown();
+    1659             :     }
+    1660             :   }
+    1661             : 
+    1662          42 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1663             : 
+    1664             :   {
+    1665          42 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1666             : 
+    1667          42 :     if (idx) {
+    1668          42 :       _failsafe_controller_idx_ = idx.value();
+    1669             :     } else {
+    1670           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1671           0 :       ros::shutdown();
+    1672             :     }
+    1673             :   }
+    1674             : 
+    1675             :   {
+    1676          42 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1677             : 
+    1678          42 :     if (idx) {
+    1679          42 :       _eland_controller_idx_ = idx.value();
+    1680             :     } else {
+    1681           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1682           0 :       ros::shutdown();
+    1683             :     }
+    1684             :   }
+    1685             : 
+    1686             :   {
+    1687          42 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1688             : 
+    1689          42 :     if (idx) {
+    1690          42 :       _joystick_controller_idx_ = idx.value();
+    1691             :     } else {
+    1692           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1693           0 :       ros::shutdown();
+    1694             :     }
+    1695             :   }
+    1696             : 
+    1697          42 :   if (_bumper_switch_controller_) {
+    1698             : 
+    1699          42 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1700             : 
+    1701          42 :     if (!idx) {
+    1702           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1703           0 :       ros::shutdown();
+    1704             :     }
+    1705             :   }
+    1706             : 
+    1707             :   {
+    1708          42 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1709             : 
+    1710          42 :     if (idx) {
+    1711          42 :       _joystick_fallback_controller_idx_ = idx.value();
+    1712             :     } else {
+    1713           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1714           0 :       ros::shutdown();
+    1715             :     }
+    1716             :   }
+    1717             : 
+    1718             :   // --------------------------------------------------------------
+    1719             :   // |                  activate the NullTracker                  |
+    1720             :   // --------------------------------------------------------------
+    1721             : 
+    1722          42 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1723             : 
+    1724          42 :   tracker_list_[_null_tracker_idx_]->activate(last_tracker_cmd_);
+    1725          42 :   active_tracker_idx_ = _null_tracker_idx_;
+    1726             : 
+    1727             :   // --------------------------------------------------------------
+    1728             :   // |    activate the eland controller as the first controller   |
+    1729             :   // --------------------------------------------------------------
+    1730             : 
+    1731          42 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_[_eland_controller_idx_].c_str());
+    1732             : 
+    1733          42 :   controller_list_[_eland_controller_idx_]->activate(last_control_output_);
+    1734          42 :   active_controller_idx_ = _eland_controller_idx_;
+    1735             : 
+    1736             :   // update the time
+    1737             :   {
+    1738          84 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1739             : 
+    1740          42 :     controller_tracker_switch_time_ = ros::Time::now();
+    1741             :   }
+    1742             : 
+    1743          42 :   output_enabled_ = false;
+    1744             : 
+    1745             :   // | --------------- set the default constraints -------------- |
+    1746             : 
+    1747          42 :   sanitized_constraints_ = current_constraints_;
+    1748          42 :   setConstraints(current_constraints_);
+    1749             : 
+    1750             :   // | ------------------------ profiler ------------------------ |
+    1751             : 
+    1752          42 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1753             : 
+    1754             :   // | ----------------------- publishers ----------------------- |
+    1755             : 
+    1756          42 :   control_output_publisher_ = OutputPublisher(nh_);
+    1757             : 
+    1758          42 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1759          42 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1760          42 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1761          42 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1762          42 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1763          42 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1764          42 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1765          42 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1766          42 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1767          42 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1768          42 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1769          42 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1770          42 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1771          42 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1772          42 :   ph_bumper_status_                      = mrs_lib::PublisherHandler<mrs_msgs::BumperStatus>(nh_, "bumper_status_out", 1);
+    1773          42 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1774          42 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1775          42 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1776          42 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1777          42 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1778             : 
+    1779             :   // | ----------------------- subscribers ---------------------- |
+    1780             : 
+    1781          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1782          42 :   shopts.nh                 = nh_;
+    1783          42 :   shopts.node_name          = "ControlManager";
+    1784          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1785          42 :   shopts.threadsafe         = true;
+    1786          42 :   shopts.autostart          = true;
+    1787          42 :   shopts.queue_size         = 10;
+    1788          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1789             : 
+    1790          42 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1791          42 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1792           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1793           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1794             :   }
+    1795             : 
+    1796          42 :   if (_odometry_innovation_check_enabled_) {
+    1797          42 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1798             :   }
+    1799             : 
+    1800          42 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1801          42 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1802          42 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1803          42 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1804          42 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1805             : 
+    1806          42 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1807             : 
+    1808             :   // | -------------------- general services -------------------- |
+    1809             : 
+    1810          42 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1811          42 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1812          42 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1813          42 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1814          42 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1815          42 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1816          42 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1817          42 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1818          42 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1819          42 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1820          42 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1821          42 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1822          42 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1823          42 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1824          42 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1825          42 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1826          42 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1827          42 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1828          42 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1829          42 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1830          42 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1831          42 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1832          42 :   service_server_validate_reference_list_    = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+    1833          42 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1834          42 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1835          42 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1836          42 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1837             : 
+    1838          42 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1839          42 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1840          42 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1841          42 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1842          42 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1843          42 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1844             : 
+    1845             :   // | ---------------- setpoint command services --------------- |
+    1846             : 
+    1847             :   // human callable
+    1848          42 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1849          42 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1850          42 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1851          42 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1852          42 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1853          42 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1854             : 
+    1855          42 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1856          42 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1857             : 
+    1858          42 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1859             :   sh_velocity_reference_ =
+    1860          42 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1861             : 
+    1862          42 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1863             :   sh_trajectory_reference_ =
+    1864          42 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1865             : 
+    1866             :   // | --------------------- other services --------------------- |
+    1867             : 
+    1868          42 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1869          42 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1870             : 
+    1871             :   // | ------------------------- timers ------------------------- |
+    1872             : 
+    1873          42 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1874          42 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1875          42 :   timer_bumper_    = nh_.createTimer(ros::Rate(_bumper_timer_rate_), &ControlManager::timerBumper, this, false, bumper_enabled_);
+    1876          42 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1877          42 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1878          42 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1879          42 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1880             : 
+    1881             :   // | ----------------------- finish init ---------------------- |
+    1882             : 
+    1883          42 :   if (!param_loader.loadedSuccessfully()) {
+    1884           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1885           0 :     ros::shutdown();
+    1886             :   }
+    1887             : 
+    1888          42 :   is_initialized_ = true;
+    1889             : 
+    1890          42 :   ROS_INFO("[ControlManager]: initialized");
+    1891          42 : }
+    1892             : 
+    1893             : //}
+    1894             : 
+    1895             : // --------------------------------------------------------------
+    1896             : // |                           timers                           |
+    1897             : // --------------------------------------------------------------
+    1898             : 
+    1899             : /* timerHwApiCapabilities() //{ */
+    1900             : 
+    1901          68 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1902             : 
+    1903         136 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1904         136 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1905             : 
+    1906          68 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1907          26 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1908          26 :     return;
+    1909             :   }
+    1910             : 
+    1911          84 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1912             : 
+    1913          42 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1914             : 
+    1915          42 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1916           2 :     ROS_INFO("[ControlManager]: - actuator command");
+    1917           2 :     _hw_api_inputs_.actuators = true;
+    1918             :   }
+    1919             : 
+    1920          42 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1921           2 :     ROS_INFO("[ControlManager]: - control group command");
+    1922           2 :     _hw_api_inputs_.control_group = true;
+    1923             :   }
+    1924             : 
+    1925          42 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1926          26 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1927          26 :     _hw_api_inputs_.attitude_rate = true;
+    1928             :   }
+    1929             : 
+    1930          42 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1931          26 :     ROS_INFO("[ControlManager]: - attitude command");
+    1932          26 :     _hw_api_inputs_.attitude = true;
+    1933             :   }
+    1934             : 
+    1935          42 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1936           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1937           2 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1938             :   }
+    1939             : 
+    1940          42 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1941           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1942           2 :     _hw_api_inputs_.acceleration_hdg = true;
+    1943             :   }
+    1944             : 
+    1945          42 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1946           2 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1947           2 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1948             :   }
+    1949             : 
+    1950          42 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1951           2 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1952           2 :     _hw_api_inputs_.velocity_hdg = true;
+    1953             :   }
+    1954             : 
+    1955          42 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1956           2 :     ROS_INFO("[ControlManager]: - position command");
+    1957           2 :     _hw_api_inputs_.position = true;
+    1958             :   }
+    1959             : 
+    1960          42 :   initialize();
+    1961             : 
+    1962          42 :   timer_hw_api_capabilities_.stop();
+    1963             : }
+    1964             : 
+    1965             : //}
+    1966             : 
+    1967             : /* //{ timerStatus() */
+    1968             : 
+    1969        4862 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1970             : 
+    1971        4862 :   if (!is_initialized_)
+    1972           0 :     return;
+    1973             : 
+    1974       14586 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1975       14586 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1976             : 
+    1977             :   // copy member variables
+    1978        9724 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1979        9724 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1980        9724 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    1981        4862 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    1982        4862 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    1983        4862 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    1984        4862 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    1985             : 
+    1986             :   double uav_x, uav_y, uav_z;
+    1987        4862 :   uav_x = uav_state.pose.position.x;
+    1988        4862 :   uav_y = uav_state.pose.position.y;
+    1989        4862 :   uav_z = uav_state.pose.position.z;
+    1990             : 
+    1991             :   // --------------------------------------------------------------
+    1992             :   // |                      print the status                      |
+    1993             :   // --------------------------------------------------------------
+    1994             : 
+    1995             :   {
+    1996        9724 :     std::string controller = _controller_names_[active_controller_idx];
+    1997        9724 :     std::string tracker    = _tracker_names_[active_tracker_idx];
+    1998        4862 :     double      mass       = last_control_output.diagnostics.total_mass;
+    1999        4862 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    2000        4862 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    2001        4862 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    2002        4862 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    2003             : 
+    2004        4862 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    2005             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2006             :   }
+    2007             : 
+    2008             :   // --------------------------------------------------------------
+    2009             :   // |                   publish the diagnostics                  |
+    2010             :   // --------------------------------------------------------------
+    2011             : 
+    2012        4862 :   publishDiagnostics();
+    2013             : 
+    2014             :   // --------------------------------------------------------------
+    2015             :   // |                publish if the offboard is on               |
+    2016             :   // --------------------------------------------------------------
+    2017             : 
+    2018        4862 :   if (offboard_mode_) {
+    2019             : 
+    2020        3308 :     std_msgs::Empty offboard_on_out;
+    2021             : 
+    2022        3308 :     ph_offboard_on_.publish(offboard_on_out);
+    2023             :   }
+    2024             : 
+    2025             :   // --------------------------------------------------------------
+    2026             :   // |                   publish the tilt error                   |
+    2027             :   // --------------------------------------------------------------
+    2028             :   {
+    2029        9724 :     std::scoped_lock lock(mutex_attitude_error_);
+    2030             : 
+    2031        4862 :     if (tilt_error_) {
+    2032             : 
+    2033        9724 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2034        4862 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2035        4862 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2036        4862 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2037             : 
+    2038        4862 :       ph_tilt_error_.publish(tilt_error_out);
+    2039             :     }
+    2040             :   }
+    2041             : 
+    2042             :   // --------------------------------------------------------------
+    2043             :   // |                  publish the control error                 |
+    2044             :   // --------------------------------------------------------------
+    2045             : 
+    2046        4862 :   if (position_error) {
+    2047             : 
+    2048        3007 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2049             : 
+    2050        6014 :     mrs_msgs::ControlError msg_out;
+    2051             : 
+    2052        3007 :     msg_out.header.stamp    = ros::Time::now();
+    2053        3007 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2054             : 
+    2055        3007 :     msg_out.position_errors.x    = pos_error_value[0];
+    2056        3007 :     msg_out.position_errors.y    = pos_error_value[1];
+    2057        3007 :     msg_out.position_errors.z    = pos_error_value[2];
+    2058        3007 :     msg_out.total_position_error = pos_error_value.norm();
+    2059             : 
+    2060        3007 :     if (yaw_error_) {
+    2061        3007 :       msg_out.yaw_error = yaw_error.value();
+    2062             :     }
+    2063             : 
+    2064        3007 :     std::map<std::string, ControllerParams>::iterator it;
+    2065             : 
+    2066        3007 :     it = controllers_.find(_controller_names_[active_controller_idx]);
+    2067             : 
+    2068        3007 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2069        3007 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2070             : 
+    2071        3007 :     ph_control_error_.publish(msg_out);
+    2072             :   }
+    2073             : 
+    2074             :   // --------------------------------------------------------------
+    2075             :   // |                  publish the mass estimate                 |
+    2076             :   // --------------------------------------------------------------
+    2077             : 
+    2078        4862 :   if (last_control_output.diagnostics.mass_estimator) {
+    2079             : 
+    2080        2757 :     std_msgs::Float64 mass_estimate_out;
+    2081        2757 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2082             : 
+    2083        2757 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2084             :   }
+    2085             : 
+    2086             :   // --------------------------------------------------------------
+    2087             :   // |                 publish the current heading                |
+    2088             :   // --------------------------------------------------------------
+    2089             : 
+    2090        4862 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2091             : 
+    2092             :     try {
+    2093             : 
+    2094             :       double heading;
+    2095             : 
+    2096        3869 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2097             : 
+    2098        7738 :       mrs_msgs::Float64Stamped heading_out;
+    2099        3869 :       heading_out.header = uav_state.header;
+    2100        3869 :       heading_out.value  = heading;
+    2101             : 
+    2102        3869 :       ph_heading_.publish(heading_out);
+    2103             :     }
+    2104           0 :     catch (...) {
+    2105           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2106             :     }
+    2107             :   }
+    2108             : 
+    2109             :   // --------------------------------------------------------------
+    2110             :   // |                  publish the current speed                 |
+    2111             :   // --------------------------------------------------------------
+    2112             : 
+    2113        4862 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2114             : 
+    2115        3869 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2116             : 
+    2117        7738 :     mrs_msgs::Float64Stamped speed_out;
+    2118        3869 :     speed_out.header = uav_state.header;
+    2119        3869 :     speed_out.value  = speed;
+    2120             : 
+    2121        3869 :     ph_speed_.publish(speed_out);
+    2122             :   }
+    2123             : 
+    2124             :   // --------------------------------------------------------------
+    2125             :   // |               publish the safety area markers              |
+    2126             :   // --------------------------------------------------------------
+    2127             : 
+    2128        4862 :   if (use_safety_area_) {
+    2129             : 
+    2130        8804 :     mrs_msgs::ReferenceStamped temp_ref;
+    2131        4402 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2132             : 
+    2133        8804 :     geometry_msgs::TransformStamped tf;
+    2134             : 
+    2135       13206 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2136             : 
+    2137        4402 :     if (ret) {
+    2138             : 
+    2139        3455 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2140             : 
+    2141        6910 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2142        6910 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2143             : 
+    2144        6910 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2145             : 
+    2146        6910 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2147        6910 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2148             : 
+    2149        6910 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2150        6910 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2151             : 
+    2152             :       // if we fail in transforming the area at some point
+    2153             :       // do not publish it at all
+    2154        3455 :       bool tf_success = true;
+    2155             : 
+    2156        6910 :       geometry_msgs::TransformStamped tf = ret.value();
+    2157             : 
+    2158             :       /* transform area points to local origin //{ */
+    2159             : 
+    2160             :       // transform border bottom points to local origin
+    2161       17275 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2162             : 
+    2163       13820 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2164       13820 :         temp_ref.header.stamp         = ros::Time(0);
+    2165       13820 :         temp_ref.reference.position.x = border_points_bot_original[i].x;
+    2166       13820 :         temp_ref.reference.position.y = border_points_bot_original[i].y;
+    2167       13820 :         temp_ref.reference.position.z = border_points_bot_original[i].z;
+    2168             : 
+    2169       27640 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2170             : 
+    2171       13820 :           temp_ref = ret.value();
+    2172             : 
+    2173       13820 :           border_points_bot_transformed[i].x = temp_ref.reference.position.x;
+    2174       13820 :           border_points_bot_transformed[i].y = temp_ref.reference.position.y;
+    2175       13820 :           border_points_bot_transformed[i].z = temp_ref.reference.position.z;
+    2176             : 
+    2177             :         } else {
+    2178           0 :           tf_success = false;
+    2179             :         }
+    2180             :       }
+    2181             : 
+    2182             :       // transform border top points to local origin
+    2183       17275 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2184             : 
+    2185       13820 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2186       13820 :         temp_ref.header.stamp         = ros::Time(0);
+    2187       13820 :         temp_ref.reference.position.x = border_points_top_original[i].x;
+    2188       13820 :         temp_ref.reference.position.y = border_points_top_original[i].y;
+    2189       13820 :         temp_ref.reference.position.z = border_points_top_original[i].z;
+    2190             : 
+    2191       27640 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2192             : 
+    2193       13820 :           temp_ref = ret.value();
+    2194             : 
+    2195       13820 :           border_points_top_transformed[i].x = temp_ref.reference.position.x;
+    2196       13820 :           border_points_top_transformed[i].y = temp_ref.reference.position.y;
+    2197       13820 :           border_points_top_transformed[i].z = temp_ref.reference.position.z;
+    2198             : 
+    2199             :         } else {
+    2200           0 :           tf_success = false;
+    2201             :         }
+    2202             :       }
+    2203             : 
+    2204             :       //}
+    2205             : 
+    2206        6910 :       visualization_msgs::Marker safety_area_marker;
+    2207             : 
+    2208        3455 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2209        3455 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2210        3455 :       safety_area_marker.color.a         = 0.15;
+    2211        3455 :       safety_area_marker.scale.x         = 0.2;
+    2212        3455 :       safety_area_marker.color.r         = 1;
+    2213        3455 :       safety_area_marker.color.g         = 0;
+    2214        3455 :       safety_area_marker.color.b         = 0;
+    2215             : 
+    2216        3455 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2217             : 
+    2218        6910 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2219             : 
+    2220        3455 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2221        3455 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2222        3455 :       safety_area_coordinates_marker.color.a         = 1;
+    2223        3455 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2224        3455 :       safety_area_coordinates_marker.color.r         = 0;
+    2225        3455 :       safety_area_coordinates_marker.color.g         = 0;
+    2226        3455 :       safety_area_coordinates_marker.color.b         = 0;
+    2227             : 
+    2228        3455 :       safety_area_coordinates_marker.id = 0;
+    2229             : 
+    2230        3455 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2231             : 
+    2232             :       /* adding safety area points //{ */
+    2233             : 
+    2234             :       // bottom border
+    2235       17275 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2236             : 
+    2237       13820 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2238       13820 :         safety_area_marker.points.push_back(border_points_bot_transformed[(i + 1) % border_points_bot_transformed.size()]);
+    2239             : 
+    2240       27640 :         std::stringstream ss;
+    2241             : 
+    2242       13820 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2243           0 :           ss << "idx: " << i << std::endl
+    2244           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2245           0 :              << "lon: " << border_points_bot_original[i].y;
+    2246             :         } else {
+    2247       13820 :           ss << "idx: " << i << std::endl
+    2248       13820 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2249       13820 :              << "y: " << border_points_bot_original[i].y;
+    2250             :         }
+    2251             : 
+    2252       13820 :         safety_area_coordinates_marker.color.r = 0;
+    2253       13820 :         safety_area_coordinates_marker.color.g = 0;
+    2254       13820 :         safety_area_coordinates_marker.color.b = 0;
+    2255             : 
+    2256       13820 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed[i];
+    2257       13820 :         safety_area_coordinates_marker.text          = ss.str();
+    2258       13820 :         safety_area_coordinates_marker.id++;
+    2259             : 
+    2260       13820 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2261             :       }
+    2262             : 
+    2263             :       // top border + top/bot edges
+    2264       17275 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2265             : 
+    2266       13820 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2267       13820 :         safety_area_marker.points.push_back(border_points_top_transformed[(i + 1) % border_points_top_transformed.size()]);
+    2268             : 
+    2269       13820 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2270       13820 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2271             : 
+    2272       27640 :         std::stringstream ss;
+    2273             : 
+    2274       13820 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2275           0 :           ss << "idx: " << i << std::endl
+    2276           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2277           0 :              << "lon: " << border_points_bot_original[i].y;
+    2278             :         } else {
+    2279       13820 :           ss << "idx: " << i << std::endl
+    2280       13820 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2281       13820 :              << "y: " << border_points_bot_original[i].y;
+    2282             :         }
+    2283             : 
+    2284       13820 :         safety_area_coordinates_marker.color.r = 1;
+    2285       13820 :         safety_area_coordinates_marker.color.g = 1;
+    2286       13820 :         safety_area_coordinates_marker.color.b = 1;
+    2287             : 
+    2288       13820 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed[i];
+    2289       13820 :         safety_area_coordinates_marker.text          = ss.str();
+    2290       13820 :         safety_area_coordinates_marker.id++;
+    2291             : 
+    2292       13820 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2293             :       }
+    2294             : 
+    2295             :       //}
+    2296             : 
+    2297        3455 :       if (tf_success) {
+    2298             : 
+    2299        3455 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2300             : 
+    2301        3455 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2302             : 
+    2303        3455 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2304             :       }
+    2305             : 
+    2306             :     } else {
+    2307         947 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2308             :     }
+    2309             :   }
+    2310             : 
+    2311             :   // --------------------------------------------------------------
+    2312             :   // |              publish the disturbances markers              |
+    2313             :   // --------------------------------------------------------------
+    2314             : 
+    2315        4862 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2316             : 
+    2317        5586 :     visualization_msgs::MarkerArray msg_out;
+    2318             : 
+    2319        2793 :     double id = 0;
+    2320             : 
+    2321        2793 :     double multiplier = 1.0;
+    2322             : 
+    2323        2793 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2324             : 
+    2325        2793 :     Eigen::Vector3d      vec3d;
+    2326        2793 :     geometry_msgs::Point point;
+    2327             : 
+    2328             :     /* world disturbance //{ */
+    2329             :     {
+    2330             : 
+    2331        5586 :       visualization_msgs::Marker marker;
+    2332             : 
+    2333        2793 :       marker.header.frame_id = uav_state.header.frame_id;
+    2334        2793 :       marker.header.stamp    = ros::Time::now();
+    2335        2793 :       marker.ns              = "control_manager";
+    2336        2793 :       marker.id              = id++;
+    2337        2793 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2338        2793 :       marker.action          = visualization_msgs::Marker::ADD;
+    2339             : 
+    2340             :       /* position //{ */
+    2341             : 
+    2342        2793 :       marker.pose.position.x = 0.0;
+    2343        2793 :       marker.pose.position.y = 0.0;
+    2344        2793 :       marker.pose.position.z = 0.0;
+    2345             : 
+    2346             :       //}
+    2347             : 
+    2348             :       /* orientation //{ */
+    2349             : 
+    2350        2793 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2351             : 
+    2352             :       //}
+    2353             : 
+    2354             :       /* origin //{ */
+    2355        2793 :       point.x = uav_x;
+    2356        2793 :       point.y = uav_y;
+    2357        2793 :       point.z = uav_z;
+    2358             : 
+    2359        2793 :       marker.points.push_back(point);
+    2360             : 
+    2361             :       //}
+    2362             : 
+    2363             :       /* tip //{ */
+    2364             : 
+    2365        2793 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2366        2793 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2367        2793 :       point.z = uav_z;
+    2368             : 
+    2369        2793 :       marker.points.push_back(point);
+    2370             : 
+    2371             :       //}
+    2372             : 
+    2373        2793 :       marker.scale.x = 0.05;
+    2374        2793 :       marker.scale.y = 0.05;
+    2375        2793 :       marker.scale.z = 0.05;
+    2376             : 
+    2377        2793 :       marker.color.a = 0.5;
+    2378        2793 :       marker.color.r = 1.0;
+    2379        2793 :       marker.color.g = 0.0;
+    2380        2793 :       marker.color.b = 0.0;
+    2381             : 
+    2382        2793 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2383             : 
+    2384        2793 :       msg_out.markers.push_back(marker);
+    2385             :     }
+    2386             : 
+    2387             :     //}
+    2388             : 
+    2389             :     /* body disturbance //{ */
+    2390             :     {
+    2391             : 
+    2392        5586 :       visualization_msgs::Marker marker;
+    2393             : 
+    2394        2793 :       marker.header.frame_id = uav_state.header.frame_id;
+    2395        2793 :       marker.header.stamp    = ros::Time::now();
+    2396        2793 :       marker.ns              = "control_manager";
+    2397        2793 :       marker.id              = id++;
+    2398        2793 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2399        2793 :       marker.action          = visualization_msgs::Marker::ADD;
+    2400             : 
+    2401             :       /* position //{ */
+    2402             : 
+    2403        2793 :       marker.pose.position.x = 0.0;
+    2404        2793 :       marker.pose.position.y = 0.0;
+    2405        2793 :       marker.pose.position.z = 0.0;
+    2406             : 
+    2407             :       //}
+    2408             : 
+    2409             :       /* orientation //{ */
+    2410             : 
+    2411        2793 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2412             : 
+    2413             :       //}
+    2414             : 
+    2415             :       /* origin //{ */
+    2416             : 
+    2417        2793 :       point.x = uav_x;
+    2418        2793 :       point.y = uav_y;
+    2419        2793 :       point.z = uav_z;
+    2420             : 
+    2421        2793 :       marker.points.push_back(point);
+    2422             : 
+    2423             :       //}
+    2424             : 
+    2425             :       /* tip //{ */
+    2426             : 
+    2427        2793 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2428        2793 :       vec3d = quat_eigen * vec3d;
+    2429             : 
+    2430        2793 :       point.x = uav_x + vec3d[0];
+    2431        2793 :       point.y = uav_y + vec3d[1];
+    2432        2793 :       point.z = uav_z + vec3d[2];
+    2433             : 
+    2434        2793 :       marker.points.push_back(point);
+    2435             : 
+    2436             :       //}
+    2437             : 
+    2438        2793 :       marker.scale.x = 0.05;
+    2439        2793 :       marker.scale.y = 0.05;
+    2440        2793 :       marker.scale.z = 0.05;
+    2441             : 
+    2442        2793 :       marker.color.a = 0.5;
+    2443        2793 :       marker.color.r = 0.0;
+    2444        2793 :       marker.color.g = 1.0;
+    2445        2793 :       marker.color.b = 0.0;
+    2446             : 
+    2447        2793 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2448             : 
+    2449        2793 :       msg_out.markers.push_back(marker);
+    2450             :     }
+    2451             : 
+    2452             :     //}
+    2453             : 
+    2454        2793 :     ph_disturbances_markers_.publish(msg_out);
+    2455             :   }
+    2456             : 
+    2457             :   // --------------------------------------------------------------
+    2458             :   // |               publish the current constraints              |
+    2459             :   // --------------------------------------------------------------
+    2460             : 
+    2461        4862 :   if (got_constraints_) {
+    2462             : 
+    2463        3835 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2464             : 
+    2465        3835 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2466             : 
+    2467        3835 :     ph_current_constraints_.publish(constraints);
+    2468             :   }
+    2469             : }
+    2470             : 
+    2471             : //}
+    2472             : 
+    2473             : /* //{ timerSafety() */
+    2474             : 
+    2475       95245 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2476             : 
+    2477       95246 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2478             : 
+    2479       95245 :   if (!is_initialized_)
+    2480           0 :     return;
+    2481             : 
+    2482      190491 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2483      190491 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2484             : 
+    2485             :   // copy member variables
+    2486       95246 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2487       95246 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2488       95246 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2489       95245 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2490       95245 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2491             : 
+    2492      178521 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2493       83276 :       active_tracker_idx == _null_tracker_idx_) {
+    2494       29928 :     return;
+    2495             :   }
+    2496             : 
+    2497       65317 :   if (odometry_switch_in_progress_) {
+    2498           0 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2499           0 :     return;
+    2500             :   }
+    2501             : 
+    2502             :   // | ------------------------ timeouts ------------------------ |
+    2503             : 
+    2504       65317 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2505       65317 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2506             : 
+    2507       65317 :     if (missing_for > _uav_state_max_missing_time_) {
+    2508           0 :       timeoutUavState(missing_for);
+    2509             :     }
+    2510             :   }
+    2511             : 
+    2512       65317 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2513           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2514             : 
+    2515           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2516           0 :       timeoutUavState(missing_for);
+    2517             :     }
+    2518             :   }
+    2519             : 
+    2520             :   // | -------------- eland and failsafe thresholds ------------- |
+    2521             : 
+    2522       65317 :   std::map<std::string, ControllerParams>::iterator it;
+    2523       65317 :   it = controllers_.find(_controller_names_[active_controller_idx]);
+    2524             : 
+    2525       65317 :   _eland_threshold_               = it->second.eland_threshold;
+    2526       65317 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2527       65317 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2528             : 
+    2529             :   // | --------- calculate control errors and tilt angle -------- |
+    2530             : 
+    2531             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2532             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2533       65317 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2534          76 :     return;
+    2535             :   }
+    2536             : 
+    2537             :   {
+    2538       65241 :     std::scoped_lock lock(mutex_attitude_error_);
+    2539             : 
+    2540       65241 :     tilt_error_ = 0;
+    2541       65241 :     yaw_error_  = 0;
+    2542             :   }
+    2543             : 
+    2544             :   {
+    2545             :     // TODO this whole scope is very clumsy
+    2546             : 
+    2547       65241 :     position_error_ = {};
+    2548             : 
+    2549       65241 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2550             : 
+    2551       64532 :       std::scoped_lock lock(mutex_position_error_);
+    2552             : 
+    2553       64532 :       if (!position_error_) {
+    2554       64527 :         position_error_ = Eigen::Vector3d::Zero(3);
+    2555             :       }
+    2556             : 
+    2557       64532 :       position_error_.value()[0] = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2558       64532 :       position_error_.value()[1] = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2559             :     }
+    2560             : 
+    2561       65241 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2562             : 
+    2563       64533 :       std::scoped_lock lock(mutex_position_error_);
+    2564             : 
+    2565       64532 :       if (!position_error_) {
+    2566           1 :         position_error_ = Eigen::Vector3d::Zero(3);
+    2567             :       }
+    2568             : 
+    2569       64532 :       position_error_.value()[2] = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2570             :     }
+    2571             :   }
+    2572             : 
+    2573             :   // rotate the drone's z axis
+    2574       65240 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2575       65240 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2576             : 
+    2577             :   // calculate the angle between the drone's z axis and the world's z axis
+    2578       65240 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2579             : 
+    2580             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2581             : 
+    2582             :   // | --------------------- the tilt error --------------------- |
+    2583             : 
+    2584       65240 :   if (last_control_output.desired_orientation) {
+    2585             : 
+    2586             :     // calculate the desired drone's z axis in the world frame
+    2587       58836 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2588       58836 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2589             : 
+    2590             :     {
+    2591       58836 :       std::scoped_lock lock(mutex_attitude_error_);
+    2592             : 
+    2593             :       // calculate the angle between the drone's z axis and the world's z axis
+    2594       58836 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2595             : 
+    2596             :       // calculate the yaw error
+    2597       58836 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2598       58836 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2599             :     }
+    2600             :   }
+    2601             : 
+    2602       65240 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2603       65240 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2604             : 
+    2605             :   // --------------------------------------------------------------
+    2606             :   // |   activate the failsafe controller in case of large error  |
+    2607             :   // --------------------------------------------------------------
+    2608             : 
+    2609       65240 :   if (position_error) {
+    2610             : 
+    2611       64531 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2612             : 
+    2613           1 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2614             : 
+    2615           1 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2616             : 
+    2617           1 :         if (!failsafe_triggered_) {
+    2618             : 
+    2619           1 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2620             :                     _failsafe_threshold_, position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2621             : 
+    2622           1 :           failsafe();
+    2623             :         }
+    2624             :       }
+    2625             :     }
+    2626             :   }
+    2627             : 
+    2628             :   // --------------------------------------------------------------
+    2629             :   // |     activate emergency land in case of large innovation    |
+    2630             :   // --------------------------------------------------------------
+    2631             : 
+    2632       65240 :   if (_odometry_innovation_check_enabled_) {
+    2633             :     {
+    2634       65240 :       auto [x, y, z] = mrs_lib::getPosition(sh_odometry_innovation_.getMsg());
+    2635             : 
+    2636       65240 :       double heading = 0;
+    2637             :       try {
+    2638       65240 :         heading = mrs_lib::getHeading(sh_odometry_innovation_.getMsg());
+    2639             :       }
+    2640           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2641           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2642             :       }
+    2643             : 
+    2644       65240 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2645             : 
+    2646       65240 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2647             : 
+    2648          74 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2649             : 
+    2650          74 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2651             : 
+    2652          53 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2653             : 
+    2654           1 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2655             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2656             : 
+    2657           1 :             eland();
+    2658             :           }
+    2659             :         }
+    2660             :       }
+    2661             :     }
+    2662             :   }
+    2663             : 
+    2664             :   // --------------------------------------------------------------
+    2665             :   // |   activate emergency land in case of medium control error  |
+    2666             :   // --------------------------------------------------------------
+    2667             : 
+    2668             :   // | ------------------- tilt control error ------------------- |
+    2669             : 
+    2670       65240 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2671             : 
+    2672           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2673             : 
+    2674           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2675             : 
+    2676           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2677             : 
+    2678           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2679             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2680             : 
+    2681           0 :         eland();
+    2682             :       }
+    2683             :     }
+    2684             :   }
+    2685             : 
+    2686             :   // | ----------------- position control error ----------------- |
+    2687             : 
+    2688       65240 :   if (position_error) {
+    2689             : 
+    2690       64531 :     double error_size = position_error->norm();
+    2691             : 
+    2692       64531 :     if (error_size > _eland_threshold_ / 2.0) {
+    2693             : 
+    2694         518 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2695             : 
+    2696         518 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2697             : 
+    2698         330 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2699             : 
+    2700         168 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2701             : 
+    2702         168 :           ungripSrv();
+    2703             :         }
+    2704             :       }
+    2705             :     }
+    2706             : 
+    2707       64531 :     if (error_size > _eland_threshold_) {
+    2708             : 
+    2709          67 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2710             : 
+    2711          67 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2712             : 
+    2713          43 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2714             : 
+    2715           1 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2716             :                     position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2717             : 
+    2718           1 :           eland();
+    2719             :         }
+    2720             :       }
+    2721             :     }
+    2722             :   }
+    2723             : 
+    2724             :   // | -------------------- yaw control error ------------------- |
+    2725             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2726             : 
+    2727       65240 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2728             : 
+    2729       65240 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2730             : 
+    2731         115 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2732             : 
+    2733         115 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2734             : 
+    2735         115 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2736             : 
+    2737         115 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2738             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2739             : 
+    2740         115 :           ungripSrv();
+    2741             :         }
+    2742             :       }
+    2743             :     }
+    2744             : 
+    2745       65239 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2746             : 
+    2747           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2748             : 
+    2749           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2750             : 
+    2751           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2752             : 
+    2753           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2754             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2755             : 
+    2756           0 :           eland();
+    2757             :         }
+    2758             :       }
+    2759             :     }
+    2760             :   }
+    2761             : 
+    2762             :   // --------------------------------------------------------------
+    2763             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2764             :   // --------------------------------------------------------------
+    2765       65239 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2766             : 
+    2767           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_);
+    2768             : 
+    2769           0 :     arming(false);
+    2770             :   }
+    2771             : 
+    2772             :   // --------------------------------------------------------------
+    2773             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2774             :   // --------------------------------------------------------------
+    2775             : 
+    2776       65239 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2777             : 
+    2778       65239 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2779             : 
+    2780             :     // the time from the last controller/tracker switch
+    2781             :     // fyi: we should not
+    2782       65240 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2783             : 
+    2784             :     // if the tile error is over the threshold
+    2785             :     // && we are not ramping up during takeoff
+    2786       65240 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2787             : 
+    2788             :       // only account for the error if some time passed from the last tracker/controller switch
+    2789           0 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2790             : 
+    2791             :         // if the threshold was not exceeded before
+    2792           0 :         if (!tilt_error_disarm_over_thr_) {
+    2793             : 
+    2794           0 :           tilt_error_disarm_over_thr_ = true;
+    2795           0 :           tilt_error_disarm_time_     = ros::Time::now();
+    2796             : 
+    2797           0 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2798             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2799             : 
+    2800             :           // if it was exceeded before, just keep it
+    2801             :         } else {
+    2802             : 
+    2803           0 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2804             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2805             :         }
+    2806             : 
+    2807             :         // if the tile error is bad, but the controller just switched,
+    2808             :         // don't think its bad anymore
+    2809             :       } else {
+    2810             : 
+    2811           0 :         tilt_error_disarm_over_thr_ = false;
+    2812           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2813             :       }
+    2814             : 
+    2815             :       // if the tilt error is fine
+    2816             :     } else {
+    2817             : 
+    2818             :       // make it fine
+    2819       65240 :       tilt_error_disarm_over_thr_ = false;
+    2820       65240 :       tilt_error_disarm_time_     = ros::Time::now();
+    2821             :     }
+    2822             : 
+    2823             :     // calculate the time over the threshold
+    2824       65240 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2825             : 
+    2826             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2827       65240 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2828             : 
+    2829           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2830             : 
+    2831             :       // only when flying and not in failsafe
+    2832           0 :       if (is_flying) {
+    2833             : 
+    2834           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2835             : 
+    2836           0 :         toggleOutput(false);
+    2837           0 :         arming(false);
+    2838             :       }
+    2839             :     }
+    2840             :   }
+    2841             : 
+    2842             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2843             : 
+    2844             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2845       65240 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2846             : 
+    2847           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2848             : 
+    2849           0 :     toggleOutput(false);
+    2850             :   }
+    2851             : }  // namespace control_manager
+    2852             : 
+    2853             : //}
+    2854             : 
+    2855             : /* //{ timerEland() */
+    2856             : 
+    2857         234 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2858             : 
+    2859         234 :   if (!is_initialized_)
+    2860           0 :     return;
+    2861             : 
+    2862         468 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2863         468 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2864             : 
+    2865             :   // copy member variables
+    2866         234 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2867             : 
+    2868         234 :   if (!last_control_output.control_output) {
+    2869           0 :     return;
+    2870             :   }
+    2871             : 
+    2872         234 :   double throttle = 0;
+    2873             : 
+    2874         234 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeCmd>(last_control_output.control_output.value())) {
+    2875           0 :     throttle = std::get<mrs_msgs::HwApiAttitudeCmd>(last_control_output.control_output.value()).throttle;
+    2876         234 :   } else if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(last_control_output.control_output.value())) {
+    2877         234 :     throttle = std::get<mrs_msgs::HwApiAttitudeRateCmd>(last_control_output.control_output.value()).throttle;
+    2878           0 :   } else if (std::holds_alternative<mrs_msgs::HwApiControlGroupCmd>(last_control_output.control_output.value())) {
+    2879           0 :     throttle = std::get<mrs_msgs::HwApiControlGroupCmd>(last_control_output.control_output.value()).throttle;
+    2880             :   } else {
+    2881           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement eland timer for this control mode");
+    2882           0 :     return;
+    2883             :   }
+    2884             : 
+    2885         234 :   if (current_state_landing_ == IDLE_STATE) {
+    2886             : 
+    2887           0 :     return;
+    2888             : 
+    2889         234 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2890             : 
+    2891         234 :     if (!last_control_output.control_output) {
+    2892           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2893           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2894           0 :       return;
+    2895             :     }
+    2896             : 
+    2897             :     // recalculate the mass based on the throttle
+    2898         234 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle) / common_handlers_->g;
+    2899         234 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2900             : 
+    2901             :     // condition for automatic motor turn off
+    2902         234 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2903          66 :       if (!throttle_under_threshold_) {
+    2904             : 
+    2905           3 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2906           3 :         throttle_under_threshold_          = true;
+    2907             :       }
+    2908             : 
+    2909          66 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2910             : 
+    2911             :     } else {
+    2912         168 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2913         168 :       throttle_under_threshold_          = false;
+    2914             :     }
+    2915             : 
+    2916         234 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2917             :       // enable callbacks? ... NO
+    2918             : 
+    2919           3 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2920           3 :       toggleOutput(false);
+    2921             : 
+    2922             :       // disarm the drone
+    2923           3 :       if (_eland_disarm_enabled_) {
+    2924             : 
+    2925           3 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2926           3 :         arming(false);
+    2927             :       }
+    2928             : 
+    2929           3 :       shutdown();
+    2930             : 
+    2931           3 :       changeLandingState(IDLE_STATE);
+    2932             : 
+    2933           3 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2934             : 
+    2935           3 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2936           3 :       timer_eland_.stop();
+    2937           3 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2938             : 
+    2939             :       // we should NOT set eland_triggered_=true
+    2940             :     }
+    2941             :   }
+    2942             : }
+    2943             : 
+    2944             : //}
+    2945             : 
+    2946             : /* //{ timerFailsafe() */
+    2947             : 
+    2948        1395 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2949             : 
+    2950        1395 :   if (!is_initialized_) {
+    2951           0 :     return;
+    2952             :   }
+    2953             : 
+    2954        1395 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2955             : 
+    2956        2790 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2957        2790 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2958             : 
+    2959             :   // copy member variables
+    2960        1395 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2961             : 
+    2962        1395 :   updateControllers(uav_state);
+    2963             : 
+    2964        1395 :   publish();
+    2965             : 
+    2966        1395 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2967             : 
+    2968        1395 :   if (!last_control_output.control_output) {
+    2969           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    2970           0 :     return;
+    2971             :   }
+    2972             : 
+    2973        1395 :   auto throttle = extractThrottle(last_control_output);
+    2974             : 
+    2975        1395 :   if (!throttle) {
+    2976           0 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    2977           0 :     return;
+    2978             :   }
+    2979             : 
+    2980        1395 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2981        1395 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2982             : 
+    2983             :   // condition for automatic motor turn off
+    2984        1395 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    2985             : 
+    2986         202 :     if (!throttle_under_threshold_) {
+    2987             : 
+    2988           1 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2989           1 :       throttle_under_threshold_          = true;
+    2990             :     }
+    2991             : 
+    2992         202 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2993             : 
+    2994             :   } else {
+    2995             : 
+    2996        1193 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    2997        1193 :     throttle_under_threshold_          = false;
+    2998             :   }
+    2999             : 
+    3000             :   // condition for automatic motor turn off
+    3001        1395 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3002             : 
+    3003           1 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3004             : 
+    3005           1 :     arming(false);
+    3006             :   }
+    3007             : }
+    3008             : 
+    3009             : //}
+    3010             : 
+    3011             : /* //{ timerJoystick() */
+    3012             : 
+    3013       18481 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3014             : 
+    3015       18481 :   if (!is_initialized_) {
+    3016           0 :     return;
+    3017             :   }
+    3018             : 
+    3019       55443 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3020       55443 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3021             : 
+    3022       36962 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3023             : 
+    3024             :   // if start was pressed and held for > 3.0 s
+    3025       18481 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3026             : 
+    3027           0 :     joystick_start_press_time_ = ros::Time(0);
+    3028             : 
+    3029           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3030             :              _joystick_controller_name_.c_str());
+    3031             : 
+    3032           0 :     joystick_start_pressed_ = false;
+    3033             : 
+    3034           0 :     switchTracker(_joystick_tracker_name_);
+    3035           0 :     switchController(_joystick_controller_name_);
+    3036             :   }
+    3037             : 
+    3038             :   // if RT+LT were pressed and held for > 0.1 s
+    3039       18481 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3040             : 
+    3041           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3042             : 
+    3043           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3044             : 
+    3045           0 :     joystick_failsafe_pressed_ = false;
+    3046             : 
+    3047           0 :     failsafe();
+    3048             :   }
+    3049             : 
+    3050             :   // if joypads were pressed and held for > 0.1 s
+    3051       18481 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3052             : 
+    3053           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3054             : 
+    3055           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3056             : 
+    3057           0 :     joystick_failsafe_pressed_ = false;
+    3058             : 
+    3059           0 :     eland();
+    3060             :   }
+    3061             : 
+    3062             :   // if back was pressed and held for > 0.1 s
+    3063       18481 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3064             : 
+    3065           0 :     joystick_back_press_time_ = ros::Time(0);
+    3066             : 
+    3067             :     // activate/deactivate the joystick goto functionality
+    3068           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3069             : 
+    3070           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3071             :   }
+    3072             : 
+    3073             :   // if the GOTO functionality is enabled...
+    3074       18481 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3075             : 
+    3076           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3077             : 
+    3078             :     // create the reference
+    3079             : 
+    3080           0 :     mrs_msgs::Vec4::Request request;
+    3081             : 
+    3082           0 :     if (fabs(joystick_data->axes[_channel_pitch_]) >= 0.05 || fabs(joystick_data->axes[_channel_roll_]) >= 0.05 ||
+    3083           0 :         fabs(joystick_data->axes[_channel_heading_]) >= 0.05 || fabs(joystick_data->axes[_channel_throttle_]) >= 0.05) {
+    3084             : 
+    3085           0 :       if (_joystick_mode_ == 0) {
+    3086             : 
+    3087           0 :         request.goal[REF_X]       = _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * _joystick_carrot_distance_;
+    3088           0 :         request.goal[REF_Y]       = _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * _joystick_carrot_distance_;
+    3089           0 :         request.goal[REF_Z]       = _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_];
+    3090           0 :         request.goal[REF_HEADING] = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3091             : 
+    3092           0 :         mrs_msgs::Vec4::Response response;
+    3093             : 
+    3094           0 :         callbackGotoFcu(request, response);
+    3095             : 
+    3096           0 :       } else if (_joystick_mode_ == 1) {
+    3097             : 
+    3098           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3099             : 
+    3100           0 :         double dt = 0.2;
+    3101             : 
+    3102           0 :         trajectory.fly_now         = true;
+    3103           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3104           0 :         trajectory.use_heading     = true;
+    3105           0 :         trajectory.dt              = dt;
+    3106             : 
+    3107           0 :         mrs_msgs::Reference point;
+    3108           0 :         point.position.x = 0;
+    3109           0 :         point.position.y = 0;
+    3110           0 :         point.position.z = 0;
+    3111           0 :         point.heading    = 0;
+    3112             : 
+    3113           0 :         trajectory.points.push_back(point);
+    3114             : 
+    3115           0 :         double speed = 1.0;
+    3116             : 
+    3117           0 :         for (int i = 0; i < 50; i++) {
+    3118             : 
+    3119           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * (speed * dt);
+    3120           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * (speed * dt);
+    3121           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_] * (speed * dt);
+    3122           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3123             : 
+    3124           0 :           trajectory.points.push_back(point);
+    3125             :         }
+    3126             : 
+    3127           0 :         setTrajectoryReference(trajectory);
+    3128             :       }
+    3129             :     }
+    3130             :   }
+    3131             : 
+    3132       18481 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3133             : 
+    3134             :     // create the reference
+    3135           0 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3136             : 
+    3137           0 :     double des_x       = 0;
+    3138           0 :     double des_y       = 0;
+    3139           0 :     double des_z       = 0;
+    3140           0 :     double des_heading = 0;
+    3141             : 
+    3142           0 :     bool nothing_to_do = true;
+    3143             : 
+    3144             :     // copy member variables
+    3145           0 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3146             : 
+    3147           0 :     if (rc_channels->channels.size() < 4) {
+    3148             : 
+    3149           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3150             :                          int(rc_channels->channels.size()));
+    3151           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3152             : 
+    3153             :     } else {
+    3154             : 
+    3155           0 :       double tmp_x       = RCChannelToRange(rc_channels->channels[_rc_channel_pitch_], _rc_horizontal_speed_, 0.1);
+    3156           0 :       double tmp_y       = -RCChannelToRange(rc_channels->channels[_rc_channel_roll_], _rc_horizontal_speed_, 0.1);
+    3157           0 :       double tmp_z       = RCChannelToRange(rc_channels->channels[_rc_channel_throttle_], _rc_vertical_speed_, 0.3);
+    3158           0 :       double tmp_heading = -RCChannelToRange(rc_channels->channels[_rc_channel_heading_], _rc_heading_rate_, 0.1);
+    3159             : 
+    3160           0 :       if (abs(tmp_x) > 1e-3) {
+    3161           0 :         des_x         = tmp_x;
+    3162           0 :         nothing_to_do = false;
+    3163             :       }
+    3164             : 
+    3165           0 :       if (abs(tmp_y) > 1e-3) {
+    3166           0 :         des_y         = tmp_y;
+    3167           0 :         nothing_to_do = false;
+    3168             :       }
+    3169             : 
+    3170           0 :       if (abs(tmp_z) > 1e-3) {
+    3171           0 :         des_z         = tmp_z;
+    3172           0 :         nothing_to_do = false;
+    3173             :       }
+    3174             : 
+    3175           0 :       if (abs(tmp_heading) > 1e-3) {
+    3176           0 :         des_heading   = tmp_heading;
+    3177           0 :         nothing_to_do = false;
+    3178             :       }
+    3179             :     }
+    3180             : 
+    3181           0 :     if (!nothing_to_do) {
+    3182             : 
+    3183           0 :       request.reference.header.frame_id = "fcu_untilted";
+    3184             : 
+    3185           0 :       request.reference.reference.use_heading_rate = true;
+    3186             : 
+    3187           0 :       request.reference.reference.velocity.x   = des_x;
+    3188           0 :       request.reference.reference.velocity.y   = des_y;
+    3189           0 :       request.reference.reference.velocity.z   = des_z;
+    3190           0 :       request.reference.reference.heading_rate = des_heading;
+    3191             : 
+    3192           0 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3193             : 
+    3194             :       // disable callbacks of all trackers
+    3195           0 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3196             : 
+    3197             :       // enable the callbacks for the active tracker
+    3198           0 :       req_enable_callbacks.data = true;
+    3199             :       {
+    3200           0 :         std::scoped_lock lock(mutex_tracker_list_);
+    3201             : 
+    3202           0 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3203           0 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3204             :       }
+    3205             : 
+    3206           0 :       callbacks_enabled_ = true;
+    3207             : 
+    3208           0 :       callbackVelocityReferenceService(request, response);
+    3209             : 
+    3210           0 :       callbacks_enabled_ = false;
+    3211             : 
+    3212           0 :       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);
+    3213             : 
+    3214             :       // disable the callbacks back again
+    3215           0 :       req_enable_callbacks.data = false;
+    3216             :       {
+    3217           0 :         std::scoped_lock lock(mutex_tracker_list_);
+    3218             : 
+    3219           0 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3220           0 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3221             :       }
+    3222             :     }
+    3223             :   }
+    3224             : }
+    3225             : 
+    3226             : //}
+    3227             : 
+    3228             : /* //{ timerBumper() */
+    3229             : 
+    3230           0 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3231             : 
+    3232           0 :   if (!is_initialized_)
+    3233           0 :     return;
+    3234             : 
+    3235           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3236           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3237             : 
+    3238           0 :   if (!bumper_enabled_) {
+    3239           0 :     return;
+    3240             :   }
+    3241             : 
+    3242           0 :   if (!isFlyingNormally()) {
+    3243           0 :     return;
+    3244             :   }
+    3245             : 
+    3246           0 :   if (!got_uav_state_) {
+    3247           0 :     return;
+    3248             :   }
+    3249             : 
+    3250           0 :   if ((ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3251           0 :     return;
+    3252             :   }
+    3253             : 
+    3254             :   // --------------------------------------------------------------
+    3255             :   // |                      bumper repulsion                      |
+    3256             :   // --------------------------------------------------------------
+    3257             : 
+    3258           0 :   bumperPushFromObstacle();
+    3259             : }
+    3260             : 
+    3261             : //}
+    3262             : 
+    3263             : /* //{ timerPirouette() */
+    3264             : 
+    3265           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3266             : 
+    3267           0 :   if (!is_initialized_)
+    3268           0 :     return;
+    3269             : 
+    3270           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3271           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3272             : 
+    3273           0 :   pirouette_iterator_++;
+    3274             : 
+    3275           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3276           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3277           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3278             : 
+    3279           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3280             : 
+    3281           0 :     _pirouette_enabled_ = false;
+    3282           0 :     timer_pirouette_.stop();
+    3283             : 
+    3284           0 :     setCallbacks(true);
+    3285             : 
+    3286           0 :     return;
+    3287             :   }
+    3288             : 
+    3289             :   // set the reference
+    3290           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3291             : 
+    3292           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3293             : 
+    3294           0 :   reference_request.header.frame_id      = "";
+    3295           0 :   reference_request.header.stamp         = ros::Time(0);
+    3296           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3297           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3298           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3299           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3300             : 
+    3301             :   // enable the callbacks for the active tracker
+    3302             :   {
+    3303           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3304             : 
+    3305           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3306           0 :     req_enable_callbacks.data = true;
+    3307             : 
+    3308           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3309             : 
+    3310           0 :     callbacks_enabled_ = true;
+    3311             :   }
+    3312             : 
+    3313           0 :   setReference(reference_request);
+    3314             : 
+    3315             :   {
+    3316           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3317             : 
+    3318             :     // disable the callbacks for the active tracker
+    3319           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3320           0 :     req_enable_callbacks.data = false;
+    3321             : 
+    3322           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3323             : 
+    3324           0 :     callbacks_enabled_ = false;
+    3325             :   }
+    3326             : }
+    3327             : 
+    3328             : //}
+    3329             : 
+    3330             : // --------------------------------------------------------------
+    3331             : // |                           asyncs                           |
+    3332             : // --------------------------------------------------------------
+    3333             : 
+    3334             : /* asyncControl() //{ */
+    3335             : 
+    3336       40744 : void ControlManager::asyncControl(void) {
+    3337             : 
+    3338       40744 :   if (!is_initialized_)
+    3339           0 :     return;
+    3340             : 
+    3341       81488 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3342             : 
+    3343      122232 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3344      122232 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3345             : 
+    3346             :   // copy member variables
+    3347       81488 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3348       40744 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3349             : 
+    3350       40744 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3351             : 
+    3352             :     // run the safety timer
+    3353             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3354       39329 :     while (running_safety_timer_) {
+    3355             : 
+    3356         301 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3357         301 :       ros::Duration wait(0.001);
+    3358         301 :       wait.sleep();
+    3359             : 
+    3360         301 :       if (!running_safety_timer_) {
+    3361         291 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3362         291 :         break;
+    3363             :       }
+    3364             :     }
+    3365             : 
+    3366       39319 :     ros::TimerEvent safety_timer_event;
+    3367       39319 :     timerSafety(safety_timer_event);
+    3368             : 
+    3369       39318 :     updateTrackers();
+    3370             : 
+    3371       39318 :     updateControllers(uav_state);
+    3372             : 
+    3373       39318 :     if (got_constraints_) {
+    3374             : 
+    3375             :       // update the constraints to trackers, if need to
+    3376       38999 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3377             : 
+    3378       38999 :       if (enforce && !constraints_being_enforced_) {
+    3379             : 
+    3380          20 :         setConstraintsToTrackers(enforce.value());
+    3381          20 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3382             : 
+    3383          20 :         constraints_being_enforced_ = true;
+    3384             : 
+    3385          20 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3386             : 
+    3387          20 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3388             :                           _controller_names_[active_controller_idx].c_str());
+    3389             : 
+    3390       38979 :       } else if (!enforce && constraints_being_enforced_) {
+    3391             : 
+    3392          19 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3393             : 
+    3394          19 :         constraints_being_enforced_ = false;
+    3395             : 
+    3396          19 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3397             : 
+    3398          19 :         setConstraintsToTrackers(current_constraints);
+    3399             :       }
+    3400             :     }
+    3401             : 
+    3402       39318 :     publish();
+    3403             :   }
+    3404             : 
+    3405             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3406       40743 :   if (odometry_switch_in_progress_) {
+    3407             : 
+    3408           0 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3409           0 :     timer_safety_.start();
+    3410           0 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3411           0 :     odometry_switch_in_progress_ = false;
+    3412             : 
+    3413             :     {
+    3414           0 :       std::scoped_lock lock(mutex_uav_state_);
+    3415             : 
+    3416           0 :       ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    3417             :                uav_state.pose.position.z, uav_heading_);
+    3418             :     }
+    3419             :   }
+    3420             : }
+    3421             : 
+    3422             : //}
+    3423             : 
+    3424             : // --------------------------------------------------------------
+    3425             : // |                          callbacks                         |
+    3426             : // --------------------------------------------------------------
+    3427             : 
+    3428             : // | --------------------- topic callbacks -------------------- |
+    3429             : 
+    3430             : /* //{ callbackOdometry() */
+    3431             : 
+    3432           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3433             : 
+    3434           0 :   if (!is_initialized_)
+    3435           0 :     return;
+    3436             : 
+    3437           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3438           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3439             : 
+    3440           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3441             : 
+    3442             :   // | ------------------ check for time stamp ------------------ |
+    3443             : 
+    3444             :   {
+    3445           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3446             : 
+    3447           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3448           0 :       return;
+    3449             :     }
+    3450             :   }
+    3451             : 
+    3452             :   // | --------------------- check for nans --------------------- |
+    3453             : 
+    3454           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3455           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3456           0 :     return;
+    3457             :   }
+    3458             : 
+    3459             :   // | ---------------------- frame switch ---------------------- |
+    3460             : 
+    3461             :   /* Odometry frame switch //{ */
+    3462             : 
+    3463             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3464             : 
+    3465           0 :   mrs_msgs::UavState uav_state_odom;
+    3466             : 
+    3467           0 :   uav_state_odom.header   = odom->header;
+    3468           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3469           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3470             : 
+    3471             :   // | ----- check for change in odometry frame of reference ---- |
+    3472             : 
+    3473           0 :   if (got_uav_state_) {
+    3474             : 
+    3475           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3476             : 
+    3477           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3478             :       {
+    3479           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3480             : 
+    3481           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,
+    3482             :                  uav_state_.pose.position.z, uav_heading_);
+    3483             :       }
+    3484             : 
+    3485           0 :       odometry_switch_in_progress_ = true;
+    3486             : 
+    3487             :       // we have to stop safety timer, otherwise it will interfere
+    3488           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3489           0 :       timer_safety_.stop();
+    3490           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3491             : 
+    3492             :       // wait for the safety timer to stop if its running
+    3493           0 :       while (running_safety_timer_) {
+    3494             : 
+    3495           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3496           0 :         ros::Duration wait(0.001);
+    3497           0 :         wait.sleep();
+    3498             : 
+    3499           0 :         if (!running_safety_timer_) {
+    3500           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3501           0 :           break;
+    3502             :         }
+    3503             :       }
+    3504             : 
+    3505             :       // we have to also for the oneshot control timer to finish
+    3506           0 :       while (running_async_control_) {
+    3507             : 
+    3508           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3509           0 :         ros::Duration wait(0.001);
+    3510           0 :         wait.sleep();
+    3511             : 
+    3512           0 :         if (!running_async_control_) {
+    3513           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3514           0 :           break;
+    3515             :         }
+    3516             :       }
+    3517             : 
+    3518             :       {
+    3519           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3520             : 
+    3521           0 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(uav_state_odom);
+    3522           0 :         controller_list_[active_controller_idx_]->switchOdometrySource(uav_state_odom);
+    3523             :       }
+    3524             :     }
+    3525             :   }
+    3526             : 
+    3527             :   //}
+    3528             : 
+    3529             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3530             : 
+    3531             :   {
+    3532           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3533             : 
+    3534           0 :     previous_uav_state_ = uav_state_;
+    3535             : 
+    3536           0 :     uav_state_ = mrs_msgs::UavState();
+    3537             : 
+    3538           0 :     uav_state_.header           = odom->header;
+    3539           0 :     uav_state_.pose             = odom->pose.pose;
+    3540           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3541             : 
+    3542             :     // transform the twist into the header's frame
+    3543             :     {
+    3544             :       // the velocity from the odometry
+    3545           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3546           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3547           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3548           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3549           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3550           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3551             : 
+    3552           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3553             : 
+    3554           0 :       if (res) {
+    3555           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3556           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3557           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3558             :       } else {
+    3559           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3560             :                            odom->header.frame_id.c_str());
+    3561           0 :         return;
+    3562             :       }
+    3563             :     }
+    3564             : 
+    3565             :     // calculate the euler angles
+    3566           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3567             : 
+    3568             :     try {
+    3569           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3570             :     }
+    3571           0 :     catch (...) {
+    3572           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3573             :     }
+    3574             : 
+    3575           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3576             : 
+    3577           0 :     got_uav_state_ = true;
+    3578             :   }
+    3579             : 
+    3580             :   // run the control loop asynchronously in an OneShotTimer
+    3581             :   // but only if its not already running
+    3582           0 :   if (!running_async_control_) {
+    3583             : 
+    3584           0 :     running_async_control_ = true;
+    3585             : 
+    3586           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3587             :   }
+    3588             : }
+    3589             : 
+    3590             : //}
+    3591             : 
+    3592             : /* //{ callbackUavState() */
+    3593             : 
+    3594       46402 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3595             : 
+    3596       46402 :   if (!is_initialized_)
+    3597        3097 :     return;
+    3598             : 
+    3599       92804 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3600       92804 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3601             : 
+    3602       46402 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3603             : 
+    3604             :   // | ------------------ check for time stamp ------------------ |
+    3605             : 
+    3606             :   {
+    3607       46402 :     std::scoped_lock lock(mutex_uav_state_);
+    3608             : 
+    3609       46402 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3610        3097 :       return;
+    3611             :     }
+    3612             :   }
+    3613             : 
+    3614             :   // | --------------------- check for nans --------------------- |
+    3615             : 
+    3616       43305 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3617           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3618           0 :     return;
+    3619             :   }
+    3620             : 
+    3621             :   // | -------------------- check for hiccups ------------------- |
+    3622             : 
+    3623             :   /* hickup detection //{ */
+    3624             : 
+    3625       43305 :   double alpha               = 0.99;
+    3626       43305 :   double alpha2              = 0.666;
+    3627       43305 :   double uav_state_count_lim = 1000;
+    3628             : 
+    3629       43305 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3630             : 
+    3631             :   // belive only reasonable numbers
+    3632       43305 :   if (uav_state_dt <= 1.0) {
+    3633             : 
+    3634       43221 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3635             : 
+    3636       43221 :     if (uav_state_count_ < uav_state_count_lim) {
+    3637       32145 :       uav_state_count_++;
+    3638             :     }
+    3639             :   }
+    3640             : 
+    3641       43305 :   if (uav_state_count_ == uav_state_count_lim) {
+    3642             : 
+    3643             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3644             : 
+    3645       11096 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3646             : 
+    3647        5028 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3648             : 
+    3649        6068 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3650             : 
+    3651        6068 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3652             :     }
+    3653             : 
+    3654       11096 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3655             : 
+    3656             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3657             : 
+    3658           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3659           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3660           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3661           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3662           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3663           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3664           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3665           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3666           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3667           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3668             :     }
+    3669             :   }
+    3670             : 
+    3671             :   //}
+    3672             : 
+    3673             :   // | ---------------------- frame switch ---------------------- |
+    3674             : 
+    3675             :   /* frame switch //{ */
+    3676             : 
+    3677             :   // | ----- check for change in odometry frame of reference ---- |
+    3678             : 
+    3679       43305 :   if (got_uav_state_) {
+    3680             : 
+    3681       43263 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3682             : 
+    3683           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3684             :       {
+    3685           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3686             : 
+    3687           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,
+    3688             :                  uav_state_.pose.position.z, uav_heading_);
+    3689             :       }
+    3690             : 
+    3691           0 :       odometry_switch_in_progress_ = true;
+    3692             : 
+    3693             :       // we have to stop safety timer, otherwise it will interfere
+    3694           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3695           0 :       timer_safety_.stop();
+    3696           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3697             : 
+    3698             :       // wait for the safety timer to stop if its running
+    3699           0 :       while (running_safety_timer_) {
+    3700             : 
+    3701           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3702           0 :         ros::Duration wait(0.001);
+    3703           0 :         wait.sleep();
+    3704             : 
+    3705           0 :         if (!running_safety_timer_) {
+    3706           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3707           0 :           break;
+    3708             :         }
+    3709             :       }
+    3710             : 
+    3711             :       // we have to also for the oneshot control timer to finish
+    3712           0 :       while (running_async_control_) {
+    3713             : 
+    3714           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3715           0 :         ros::Duration wait(0.001);
+    3716           0 :         wait.sleep();
+    3717             : 
+    3718           0 :         if (!running_async_control_) {
+    3719           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3720           0 :           break;
+    3721             :         }
+    3722             :       }
+    3723             : 
+    3724             :       {
+    3725           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3726             : 
+    3727           0 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(*uav_state);
+    3728           0 :         controller_list_[active_controller_idx_]->switchOdometrySource(*uav_state);
+    3729             :       }
+    3730             :     }
+    3731             :   }
+    3732             : 
+    3733             :   //}
+    3734             : 
+    3735             :   // --------------------------------------------------------------
+    3736             :   // |           copy the UavState message for later use          |
+    3737             :   // --------------------------------------------------------------
+    3738             : 
+    3739             :   {
+    3740       43305 :     std::scoped_lock lock(mutex_uav_state_);
+    3741             : 
+    3742       43305 :     previous_uav_state_ = uav_state_;
+    3743             : 
+    3744       43305 :     uav_state_ = *uav_state;
+    3745             : 
+    3746       43305 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3747             : 
+    3748             :     try {
+    3749       43305 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3750             :     }
+    3751           0 :     catch (...) {
+    3752           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3753             :     }
+    3754             : 
+    3755       43305 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3756             : 
+    3757       43305 :     got_uav_state_ = true;
+    3758             :   }
+    3759             : 
+    3760             :   // run the control loop asynchronously in an OneShotTimer
+    3761             :   // but only if its not already running
+    3762       43305 :   if (!running_async_control_) {
+    3763             : 
+    3764       40744 :     running_async_control_ = true;
+    3765             : 
+    3766       40744 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3767             :   }
+    3768             : }
+    3769             : 
+    3770             : //}
+    3771             : 
+    3772             : /* //{ callbackGNSS() */
+    3773             : 
+    3774       29899 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3775             : 
+    3776       29899 :   if (!is_initialized_)
+    3777          57 :     return;
+    3778             : 
+    3779       89526 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3780       89526 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3781             : 
+    3782       29842 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3783             : }
+    3784             : 
+    3785             : //}
+    3786             : 
+    3787             : /* callbackJoystick() //{ */
+    3788             : 
+    3789           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3790             : 
+    3791           0 :   if (!is_initialized_)
+    3792           0 :     return;
+    3793             : 
+    3794           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3795           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3796             : 
+    3797             :   // copy member variables
+    3798           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3799           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3800             : 
+    3801           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3802             : 
+    3803             :   // TODO check if the array is smaller than the largest idx
+    3804           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3805           0 :     return;
+    3806             :   }
+    3807             : 
+    3808             :   // | ---- switching back to fallback tracker and controller --- |
+    3809             : 
+    3810             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3811           0 :   if ((joystick_data->buttons[_channel_A_] == 1 || joystick_data->buttons[_channel_B_] == 1 || joystick_data->buttons[_channel_X_] == 1 ||
+    3812           0 :        joystick_data->buttons[_channel_Y_] == 1) &&
+    3813           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3814             : 
+    3815           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3816             : 
+    3817           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3818           0 :     switchController(_joystick_fallback_controller_name_);
+    3819             : 
+    3820           0 :     joystick_goto_enabled_ = false;
+    3821             :   }
+    3822             : 
+    3823             :   // | ------- joystick control activation ------- |
+    3824             : 
+    3825             :   // if start button was pressed
+    3826           0 :   if (joystick_data->buttons[_channel_start_] == 1) {
+    3827             : 
+    3828           0 :     if (!joystick_start_pressed_) {
+    3829             : 
+    3830           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3831             : 
+    3832           0 :       joystick_start_pressed_    = true;
+    3833           0 :       joystick_start_press_time_ = ros::Time::now();
+    3834             :     }
+    3835             : 
+    3836           0 :   } else if (joystick_start_pressed_) {
+    3837             : 
+    3838           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3839             : 
+    3840           0 :     joystick_start_pressed_    = false;
+    3841           0 :     joystick_start_press_time_ = ros::Time(0);
+    3842             :   }
+    3843             : 
+    3844             :   // | ---------------- Joystick goto activation ---------------- |
+    3845             : 
+    3846             :   // if back button was pressed
+    3847           0 :   if (joystick_data->buttons[_channel_back_] == 1) {
+    3848             : 
+    3849           0 :     if (!joystick_back_pressed_) {
+    3850             : 
+    3851           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3852             : 
+    3853           0 :       joystick_back_pressed_    = true;
+    3854           0 :       joystick_back_press_time_ = ros::Time::now();
+    3855             :     }
+    3856             : 
+    3857           0 :   } else if (joystick_back_pressed_) {
+    3858             : 
+    3859           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3860             : 
+    3861           0 :     joystick_back_pressed_    = false;
+    3862           0 :     joystick_back_press_time_ = ros::Time(0);
+    3863             :   }
+    3864             : 
+    3865             :   // | ------------------------ Failsafes ----------------------- |
+    3866             : 
+    3867             :   // if LT and RT buttons are both pressed down
+    3868           0 :   if (joystick_data->axes[_channel_LT_] < -0.99 && joystick_data->axes[_channel_RT_] < -0.99) {
+    3869             : 
+    3870           0 :     if (!joystick_failsafe_pressed_) {
+    3871             : 
+    3872           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3873             : 
+    3874           0 :       joystick_failsafe_pressed_    = true;
+    3875           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3876             :     }
+    3877             : 
+    3878           0 :   } else if (joystick_failsafe_pressed_) {
+    3879             : 
+    3880           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3881             : 
+    3882           0 :     joystick_failsafe_pressed_    = false;
+    3883           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3884             :   }
+    3885             : 
+    3886             :   // if left and right joypads are both pressed down
+    3887           0 :   if (joystick_data->buttons[_channel_L_joy_] == 1 && joystick_data->buttons[_channel_R_joy_] == 1) {
+    3888             : 
+    3889           0 :     if (!joystick_eland_pressed_) {
+    3890             : 
+    3891           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3892             : 
+    3893           0 :       joystick_eland_pressed_    = true;
+    3894           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3895             :     }
+    3896             : 
+    3897           0 :   } else if (joystick_eland_pressed_) {
+    3898             : 
+    3899           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3900             : 
+    3901           0 :     joystick_eland_pressed_    = false;
+    3902           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3903             :   }
+    3904             : }
+    3905             : 
+    3906             : //}
+    3907             : 
+    3908             : /* //{ callbackHwApiStatus() */
+    3909             : 
+    3910      195281 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3911             : 
+    3912      195281 :   if (!is_initialized_)
+    3913         219 :     return;
+    3914             : 
+    3915      585186 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3916      585186 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3917             : 
+    3918      390124 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3919             : 
+    3920             :   // | ------ detect and print the changes in offboard mode ----- |
+    3921      195062 :   if (state->offboard) {
+    3922             : 
+    3923      132825 :     if (!offboard_mode_) {
+    3924          39 :       offboard_mode_          = true;
+    3925          39 :       offboard_mode_was_true_ = true;
+    3926          39 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3927             :     }
+    3928             : 
+    3929             :   } else {
+    3930             : 
+    3931       62237 :     if (offboard_mode_) {
+    3932           0 :       offboard_mode_ = false;
+    3933           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3934             :     }
+    3935             :   }
+    3936             : 
+    3937             :   // | --------- detect and print the changes in arming --------- |
+    3938      195062 :   if (state->armed == true) {
+    3939             : 
+    3940      137517 :     if (!armed_) {
+    3941          42 :       armed_ = true;
+    3942          42 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    3943             :     }
+    3944             : 
+    3945             :   } else {
+    3946             : 
+    3947       57545 :     if (armed_) {
+    3948           9 :       armed_ = false;
+    3949           9 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    3950             :     }
+    3951             :   }
+    3952             : }
+    3953             : 
+    3954             : //}
+    3955             : 
+    3956             : /* //{ callbackRC() */
+    3957             : 
+    3958        5850 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    3959             : 
+    3960        5850 :   if (!is_initialized_)
+    3961           0 :     return;
+    3962             : 
+    3963       17550 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    3964       17550 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    3965             : 
+    3966       11700 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    3967             : 
+    3968        5850 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    3969             : 
+    3970             :   // | ------------------- rc joystic control ------------------- |
+    3971             : 
+    3972             :   // when the switch change its position
+    3973        5850 :   if (_rc_goto_enabled_) {
+    3974             : 
+    3975        5850 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    3976             : 
+    3977           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    3978             :                          int(rc->channels.size()));
+    3979             : 
+    3980             :     } else {
+    3981             : 
+    3982        5850 :       bool channel_low  = rc->channels[_rc_joystick_channel_] < (0.5 - RC_DEADBAND) ? true : false;
+    3983        5850 :       bool channel_high = rc->channels[_rc_joystick_channel_] > (0.5 + RC_DEADBAND) ? true : false;
+    3984             : 
+    3985        5850 :       if (channel_low) {
+    3986        5850 :         rc_joystick_channel_was_low_ = true;
+    3987             :       }
+    3988             : 
+    3989             :       // rc control activation
+    3990        5850 :       if (!rc_goto_active_) {
+    3991             : 
+    3992        5850 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    3993             : 
+    3994           0 :           if (isFlyingNormally()) {
+    3995             : 
+    3996           0 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    3997             : 
+    3998           0 :             callbacks_enabled_ = false;
+    3999             : 
+    4000           0 :             std_srvs::SetBoolRequest req_goto_out;
+    4001           0 :             req_goto_out.data = false;
+    4002             : 
+    4003           0 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4004           0 :             req_enable_callbacks.data = callbacks_enabled_;
+    4005             : 
+    4006             :             {
+    4007           0 :               std::scoped_lock lock(mutex_tracker_list_);
+    4008             : 
+    4009             :               // disable callbacks of all trackers
+    4010           0 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4011           0 :                 tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4012             :               }
+    4013             :             }
+    4014             : 
+    4015           0 :             rc_goto_active_ = true;
+    4016             : 
+    4017             :           } else {
+    4018             : 
+    4019           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4020           0 :           }
+    4021             : 
+    4022        5850 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4023             : 
+    4024           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4025             :         }
+    4026             :       }
+    4027             : 
+    4028             :       // rc control deactivation
+    4029        5850 :       if (rc_goto_active_ && channel_low) {
+    4030             : 
+    4031           0 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4032             : 
+    4033           0 :         callbacks_enabled_ = true;
+    4034             : 
+    4035           0 :         std_srvs::SetBoolRequest req_goto_out;
+    4036           0 :         req_goto_out.data = true;
+    4037             : 
+    4038           0 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4039           0 :         req_enable_callbacks.data = callbacks_enabled_;
+    4040             : 
+    4041             :         {
+    4042           0 :           std::scoped_lock lock(mutex_tracker_list_);
+    4043             : 
+    4044             :           // enable callbacks of all trackers
+    4045           0 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4046           0 :             tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4047             :           }
+    4048             :         }
+    4049             : 
+    4050           0 :         rc_goto_active_ = false;
+    4051             :       }
+    4052             : 
+    4053             :       // do not forget to update the last... variable
+    4054             :       // only do that if its out of the deadband
+    4055        5850 :       if (channel_high || channel_low) {
+    4056        5850 :         rc_joystick_channel_last_value_ = rc->channels[_rc_joystick_channel_];
+    4057             :       }
+    4058             :     }
+    4059             :   }
+    4060             : 
+    4061             :   // | ------------------------ rc eland ------------------------ |
+    4062        5850 :   if (_rc_escalating_failsafe_enabled_) {
+    4063             : 
+    4064        5850 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4065             : 
+    4066           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4067             :                          int(rc->channels.size()));
+    4068             : 
+    4069             :     } else {
+    4070             : 
+    4071        5850 :       if (rc->channels[_rc_escalating_failsafe_channel_] >= _rc_escalating_failsafe_threshold_) {
+    4072             : 
+    4073           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4074             : 
+    4075           0 :         auto [success, message] = escalatingFailsafe();
+    4076             : 
+    4077           0 :         if (success) {
+    4078           0 :           rc_escalating_failsafe_triggered_ = true;
+    4079             :         }
+    4080             :       }
+    4081             :     }
+    4082             :   }
+    4083             : }
+    4084             : 
+    4085             : //}
+    4086             : 
+    4087             : // | --------------------- topic timeouts --------------------- |
+    4088             : 
+    4089             : /* timeoutUavState() //{ */
+    4090             : 
+    4091           0 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4092             : 
+    4093           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4094             : 
+    4095           0 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4096             : 
+    4097             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4098             :     // in place of the callbackUavState/callbackOdometry().
+    4099             : 
+    4100           0 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4101             : 
+    4102           0 :     failsafe();
+    4103             :   }
+    4104           0 : }
+    4105             : 
+    4106             : //}
+    4107             : 
+    4108             : // | -------------------- service callbacks ------------------- |
+    4109             : 
+    4110             : /* //{ callbackSwitchTracker() */
+    4111             : 
+    4112          82 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4113             : 
+    4114          82 :   if (!is_initialized_)
+    4115           0 :     return false;
+    4116             : 
+    4117          82 :   if (failsafe_triggered_ || eland_triggered_) {
+    4118             : 
+    4119           0 :     std::stringstream ss;
+    4120           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4121             : 
+    4122           0 :     res.message = ss.str();
+    4123           0 :     res.success = false;
+    4124             : 
+    4125           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4126             : 
+    4127           0 :     return true;
+    4128             :   }
+    4129             : 
+    4130          82 :   auto [success, response] = switchTracker(req.value);
+    4131             : 
+    4132          82 :   res.success = success;
+    4133          82 :   res.message = response;
+    4134             : 
+    4135          82 :   return true;
+    4136             : }
+    4137             : 
+    4138             : //}
+    4139             : 
+    4140             : /* callbackSwitchController() //{ */
+    4141             : 
+    4142          80 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4143             : 
+    4144          80 :   if (!is_initialized_)
+    4145           0 :     return false;
+    4146             : 
+    4147          80 :   if (failsafe_triggered_ || eland_triggered_) {
+    4148             : 
+    4149           0 :     std::stringstream ss;
+    4150           0 :     ss << "can not switch controller, eland or failsafe active";
+    4151             : 
+    4152           0 :     res.message = ss.str();
+    4153           0 :     res.success = false;
+    4154             : 
+    4155           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4156             : 
+    4157           0 :     return true;
+    4158             :   }
+    4159             : 
+    4160          80 :   auto [success, response] = switchController(req.value);
+    4161             : 
+    4162          80 :   res.success = success;
+    4163          80 :   res.message = response;
+    4164             : 
+    4165          80 :   return true;
+    4166             : }
+    4167             : 
+    4168             : //}
+    4169             : 
+    4170             : /* //{ callbackSwitchTracker() */
+    4171             : 
+    4172           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4173             : 
+    4174           0 :   if (!is_initialized_)
+    4175           0 :     return false;
+    4176             : 
+    4177           0 :   std::stringstream message;
+    4178             : 
+    4179           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4180             : 
+    4181           0 :     message << "can not reset tracker, eland or failsafe active";
+    4182             : 
+    4183           0 :     res.message = message.str();
+    4184           0 :     res.success = false;
+    4185             : 
+    4186           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4187             : 
+    4188           0 :     return true;
+    4189             :   }
+    4190             : 
+    4191             :   // reactivate the current tracker
+    4192             :   {
+    4193           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4194             : 
+    4195           0 :     std::string tracker_name = _tracker_names_[active_tracker_idx_];
+    4196             : 
+    4197           0 :     bool succ = tracker_list_[active_tracker_idx_]->resetStatic();
+    4198             : 
+    4199           0 :     if (succ) {
+    4200           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4201           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4202             :     } else {
+    4203           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4204           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4205             :     }
+    4206             :   }
+    4207             : 
+    4208           0 :   res.message = message.str();
+    4209           0 :   res.success = true;
+    4210             : 
+    4211           0 :   return true;
+    4212             : }
+    4213             : 
+    4214             : //}
+    4215             : 
+    4216             : /* //{ callbackEHover() */
+    4217             : 
+    4218           0 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4219             : 
+    4220           0 :   if (!is_initialized_)
+    4221           0 :     return false;
+    4222             : 
+    4223           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4224             : 
+    4225           0 :     std::stringstream ss;
+    4226           0 :     ss << "can not switch controller, eland or failsafe active";
+    4227             : 
+    4228           0 :     res.message = ss.str();
+    4229           0 :     res.success = false;
+    4230             : 
+    4231           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4232             : 
+    4233           0 :     return true;
+    4234             :   }
+    4235             : 
+    4236           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4237             : 
+    4238           0 :   auto [success, message] = ehover();
+    4239             : 
+    4240           0 :   res.success = success;
+    4241           0 :   res.message = message;
+    4242             : 
+    4243           0 :   return true;
+    4244             : }
+    4245             : 
+    4246             : //}
+    4247             : 
+    4248             : /* callbackFailsafe() //{ */
+    4249             : 
+    4250           0 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4251             : 
+    4252           0 :   if (!is_initialized_)
+    4253           0 :     return false;
+    4254             : 
+    4255           0 :   if (failsafe_triggered_) {
+    4256             : 
+    4257           0 :     std::stringstream ss;
+    4258           0 :     ss << "can not activate failsafe, it is already active";
+    4259             : 
+    4260           0 :     res.message = ss.str();
+    4261           0 :     res.success = false;
+    4262             : 
+    4263           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4264             : 
+    4265           0 :     return true;
+    4266             :   }
+    4267             : 
+    4268           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4269             : 
+    4270           0 :   auto [success, message] = failsafe();
+    4271             : 
+    4272           0 :   res.success = success;
+    4273           0 :   res.message = message;
+    4274             : 
+    4275           0 :   return true;
+    4276             : }
+    4277             : 
+    4278             : //}
+    4279             : 
+    4280             : /* callbackFailsafeEscalating() //{ */
+    4281             : 
+    4282           0 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4283             : 
+    4284           0 :   if (!is_initialized_)
+    4285           0 :     return false;
+    4286             : 
+    4287           0 :   if (_service_escalating_failsafe_enabled_) {
+    4288             : 
+    4289           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4290             : 
+    4291           0 :     auto [success, message] = escalatingFailsafe();
+    4292             : 
+    4293           0 :     res.success = success;
+    4294           0 :     res.message = message;
+    4295             : 
+    4296             :   } else {
+    4297             : 
+    4298           0 :     std::stringstream ss;
+    4299           0 :     ss << "escalating failsafe is disabled";
+    4300             : 
+    4301           0 :     res.success = false;
+    4302           0 :     res.message = ss.str();
+    4303             : 
+    4304           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4305             :   }
+    4306             : 
+    4307           0 :   return true;
+    4308             : }
+    4309             : 
+    4310             : //}
+    4311             : 
+    4312             : /* //{ callbackELand() */
+    4313             : 
+    4314           1 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4315             : 
+    4316           1 :   if (!is_initialized_)
+    4317           0 :     return false;
+    4318             : 
+    4319           1 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4320             : 
+    4321           1 :   auto [success, message] = eland();
+    4322             : 
+    4323           1 :   res.success = success;
+    4324           1 :   res.message = message;
+    4325             : 
+    4326           1 :   return true;
+    4327             : }
+    4328             : 
+    4329             : //}
+    4330             : 
+    4331             : /* //{ callbackParachute() */
+    4332             : 
+    4333           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4334             : 
+    4335           0 :   if (!is_initialized_)
+    4336           0 :     return false;
+    4337             : 
+    4338           0 :   if (!_parachute_enabled_) {
+    4339             : 
+    4340           0 :     std::stringstream ss;
+    4341           0 :     ss << "parachute disabled";
+    4342           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4343           0 :     res.message = ss.str();
+    4344           0 :     res.success = false;
+    4345             :   }
+    4346             : 
+    4347           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4348             : 
+    4349           0 :   auto [success, message] = deployParachute();
+    4350             : 
+    4351           0 :   res.success = success;
+    4352           0 :   res.message = message;
+    4353             : 
+    4354           0 :   return true;
+    4355             : }
+    4356             : 
+    4357             : //}
+    4358             : 
+    4359             : /* //{ callbackToggleOutput() */
+    4360             : 
+    4361          39 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4362             : 
+    4363          39 :   if (!is_initialized_)
+    4364           0 :     return false;
+    4365             : 
+    4366          39 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4367             : 
+    4368             :   // copy member variables
+    4369          78 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4370             : 
+    4371          78 :   std::stringstream ss;
+    4372             : 
+    4373          39 :   bool prereq_check = true;
+    4374             : 
+    4375             :   {
+    4376          78 :     mrs_msgs::ReferenceStamped current_coord;
+    4377          39 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4378          39 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4379          39 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4380             : 
+    4381          39 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4382           0 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4383           0 :       prereq_check = false;
+    4384             :     }
+    4385             :   }
+    4386             : 
+    4387          39 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4388           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4389           0 :     prereq_check = false;
+    4390             :   }
+    4391             : 
+    4392          39 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4393           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4394           0 :     prereq_check = false;
+    4395             :   }
+    4396             : 
+    4397          39 :   if (bumper_enabled_ && !sh_bumper_.hasMsg()) {
+    4398           0 :     ss << "cannot toggle output ON, missing bumper data!";
+    4399           0 :     prereq_check = false;
+    4400             :   }
+    4401             : 
+    4402          39 :   if (!prereq_check) {
+    4403             : 
+    4404           0 :     res.message = ss.str();
+    4405           0 :     res.success = false;
+    4406             : 
+    4407           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4408             : 
+    4409           0 :     return false;
+    4410             : 
+    4411             :   } else {
+    4412             : 
+    4413          39 :     toggleOutput(req.data);
+    4414             : 
+    4415          39 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4416          39 :     res.message = ss.str();
+    4417          39 :     res.success = true;
+    4418             : 
+    4419          39 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4420             : 
+    4421          39 :     publishDiagnostics();
+    4422             : 
+    4423          39 :     return true;
+    4424             :   }
+    4425             : }
+    4426             : 
+    4427             : //}
+    4428             : 
+    4429             : /* callbackArm() //{ */
+    4430             : 
+    4431           2 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4432             : 
+    4433           2 :   if (!is_initialized_)
+    4434           0 :     return false;
+    4435             : 
+    4436           2 :   ROS_INFO("[ControlManager]: arming by service");
+    4437             : 
+    4438           4 :   std::stringstream ss;
+    4439             : 
+    4440           2 :   if (failsafe_triggered_ || eland_triggered_) {
+    4441             : 
+    4442           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4443             : 
+    4444           0 :     res.message = ss.str();
+    4445           0 :     res.success = false;
+    4446             : 
+    4447           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4448             : 
+    4449           0 :     return true;
+    4450             :   }
+    4451             : 
+    4452           2 :   if (req.data) {
+    4453             : 
+    4454           0 :     ss << "this service is not allowed to arm the UAV";
+    4455           0 :     res.success = false;
+    4456           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4457             : 
+    4458             :   } else {
+    4459             : 
+    4460           4 :     auto [success, message] = arming(false);
+    4461             : 
+    4462           2 :     if (success) {
+    4463             : 
+    4464           2 :       ss << "disarmed";
+    4465           2 :       res.success = true;
+    4466           2 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4467             : 
+    4468             :     } else {
+    4469             : 
+    4470           0 :       ss << "could not disarm: " << message;
+    4471           0 :       res.success = false;
+    4472           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4473             :     }
+    4474             :   }
+    4475             : 
+    4476           2 :   res.message = ss.str();
+    4477             : 
+    4478           2 :   return true;
+    4479             : }
+    4480             : 
+    4481             : //}
+    4482             : 
+    4483             : /* //{ callbackEnableCallbacks() */
+    4484             : 
+    4485           2 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4486             : 
+    4487           2 :   if (!is_initialized_)
+    4488           0 :     return false;
+    4489             : 
+    4490           2 :   setCallbacks(req.data);
+    4491             : 
+    4492           2 :   std::stringstream ss;
+    4493             : 
+    4494           2 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4495             : 
+    4496           2 :   res.message = ss.str();
+    4497           2 :   res.success = true;
+    4498             : 
+    4499           2 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4500             : 
+    4501           2 :   return true;
+    4502             : }
+    4503             : 
+    4504             : //}
+    4505             : 
+    4506             : /* callbackSetConstraints() //{ */
+    4507             : 
+    4508          42 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4509             : 
+    4510          42 :   if (!is_initialized_) {
+    4511           0 :     res.message = "not initialized";
+    4512           0 :     res.success = false;
+    4513           0 :     return true;
+    4514             :   }
+    4515             : 
+    4516             :   {
+    4517          84 :     std::scoped_lock lock(mutex_constraints_);
+    4518             : 
+    4519          42 :     current_constraints_ = req;
+    4520             : 
+    4521          42 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4522             : 
+    4523          42 :     if (enforced) {
+    4524           0 :       sanitized_constraints_      = enforced.value();
+    4525           0 :       constraints_being_enforced_ = true;
+    4526             :     } else {
+    4527          42 :       sanitized_constraints_      = req;
+    4528          42 :       constraints_being_enforced_ = false;
+    4529             :     }
+    4530             : 
+    4531          42 :     got_constraints_ = true;
+    4532             : 
+    4533          42 :     setConstraintsToControllers(current_constraints_);
+    4534          42 :     setConstraintsToTrackers(sanitized_constraints_);
+    4535             :   }
+    4536             : 
+    4537          42 :   res.message = "setting constraints";
+    4538          42 :   res.success = true;
+    4539             : 
+    4540          42 :   return true;
+    4541             : }
+    4542             : 
+    4543             : //}
+    4544             : 
+    4545             : /* //{ callbackEmergencyReference() */
+    4546             : 
+    4547           0 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4548             : 
+    4549           0 :   if (!is_initialized_)
+    4550           0 :     return false;
+    4551             : 
+    4552           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4553             : 
+    4554           0 :   callbacks_enabled_ = false;
+    4555             : 
+    4556           0 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4557             : 
+    4558           0 :   std::stringstream ss;
+    4559             : 
+    4560             :   // transform the reference to the current frame
+    4561           0 :   mrs_msgs::ReferenceStamped original_reference;
+    4562           0 :   original_reference.header    = req.header;
+    4563           0 :   original_reference.reference = req.reference;
+    4564             : 
+    4565           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4566             : 
+    4567           0 :   if (!ret) {
+    4568             : 
+    4569           0 :     ss << "the emergency reference could not be transformed";
+    4570             : 
+    4571           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4572           0 :     res.message = ss.str();
+    4573           0 :     res.success = false;
+    4574           0 :     return true;
+    4575             :   }
+    4576             : 
+    4577           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4578             : 
+    4579           0 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4580             : 
+    4581           0 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4582           0 :   req_goto_out.reference = transformed_reference.reference;
+    4583             : 
+    4584             :   {
+    4585           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4586             : 
+    4587             :     // disable callbacks of all trackers
+    4588           0 :     req_enable_callbacks.data = false;
+    4589           0 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4590           0 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4591             :     }
+    4592             : 
+    4593             :     // enable the callbacks for the active tracker
+    4594           0 :     req_enable_callbacks.data = true;
+    4595           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4596             : 
+    4597             :     // call the setReference()
+    4598           0 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    4599           0 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4600             : 
+    4601             :     // disable the callbacks back again
+    4602           0 :     req_enable_callbacks.data = false;
+    4603           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4604             : 
+    4605           0 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4606           0 :       res.message = tracker_response->message;
+    4607           0 :       res.success = tracker_response->success;
+    4608             :     } else {
+    4609           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    4610           0 :       res.message = ss.str();
+    4611           0 :       res.success = false;
+    4612             :     }
+    4613             :   }
+    4614             : 
+    4615           0 :   return true;
+    4616             : }
+    4617             : 
+    4618             : //}
+    4619             : 
+    4620             : /* callbackPirouette() //{ */
+    4621             : 
+    4622           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4623             : 
+    4624           0 :   if (!is_initialized_)
+    4625           0 :     return false;
+    4626             : 
+    4627             :   // copy member variables
+    4628           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4629             : 
+    4630             :   double uav_heading;
+    4631             :   try {
+    4632           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4633             :   }
+    4634           0 :   catch (...) {
+    4635           0 :     std::stringstream ss;
+    4636           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4637             : 
+    4638           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4639             : 
+    4640           0 :     res.message = ss.str();
+    4641           0 :     res.success = false;
+    4642             : 
+    4643           0 :     return false;
+    4644             :   }
+    4645             : 
+    4646           0 :   if (_pirouette_enabled_) {
+    4647           0 :     res.success = false;
+    4648           0 :     res.message = "already active";
+    4649           0 :     return true;
+    4650             :   }
+    4651             : 
+    4652           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4653             : 
+    4654           0 :     std::stringstream ss;
+    4655           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4656             : 
+    4657           0 :     res.message = ss.str();
+    4658           0 :     res.success = false;
+    4659             : 
+    4660           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4661             : 
+    4662           0 :     return true;
+    4663             :   }
+    4664             : 
+    4665           0 :   _pirouette_enabled_ = true;
+    4666             : 
+    4667           0 :   setCallbacks(false);
+    4668             : 
+    4669           0 :   pirouette_initial_heading_ = uav_heading;
+    4670           0 :   pirouette_iterator_        = 0;
+    4671           0 :   timer_pirouette_.start();
+    4672             : 
+    4673           0 :   res.success = true;
+    4674           0 :   res.message = "activated";
+    4675             : 
+    4676           0 :   return true;
+    4677             : }
+    4678             : 
+    4679             : //}
+    4680             : 
+    4681             : /* callbackUseJoystick() //{ */
+    4682             : 
+    4683           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4684             : 
+    4685           0 :   if (!is_initialized_) {
+    4686           0 :     return false;
+    4687             :   }
+    4688             : 
+    4689           0 :   std::stringstream ss;
+    4690             : 
+    4691             :   {
+    4692           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4693             : 
+    4694           0 :     if (!success) {
+    4695             : 
+    4696           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4697           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4698             : 
+    4699           0 :       res.success = false;
+    4700           0 :       res.message = ss.str();
+    4701             : 
+    4702           0 :       return true;
+    4703             :     }
+    4704             :   }
+    4705             : 
+    4706           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4707             : 
+    4708           0 :   if (!success) {
+    4709             : 
+    4710           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4711           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4712             : 
+    4713           0 :     res.success = false;
+    4714           0 :     res.message = ss.str();
+    4715             : 
+    4716             :     // switch back to hover tracker
+    4717           0 :     switchTracker(_ehover_tracker_name_);
+    4718             : 
+    4719             :     // switch back to safety controller
+    4720           0 :     switchController(_eland_controller_name_);
+    4721             : 
+    4722           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4723             : 
+    4724           0 :     return true;
+    4725             :   }
+    4726             : 
+    4727           0 :   ss << "switched to joystick control";
+    4728             : 
+    4729           0 :   res.success = true;
+    4730           0 :   res.message = ss.str();
+    4731             : 
+    4732           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4733             : 
+    4734           0 :   return true;
+    4735             : }
+    4736             : 
+    4737             : //}
+    4738             : 
+    4739             : /* //{ callbackHover() */
+    4740             : 
+    4741           0 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4742             : 
+    4743           0 :   if (!is_initialized_)
+    4744           0 :     return false;
+    4745             : 
+    4746           0 :   auto [success, message] = hover();
+    4747             : 
+    4748           0 :   res.success = success;
+    4749           0 :   res.message = message;
+    4750             : 
+    4751           0 :   return true;
+    4752             : }
+    4753             : 
+    4754             : //}
+    4755             : 
+    4756             : /* //{ callbackStartTrajectoryTracking() */
+    4757             : 
+    4758           0 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4759             : 
+    4760           0 :   if (!is_initialized_)
+    4761           0 :     return false;
+    4762             : 
+    4763           0 :   auto [success, message] = startTrajectoryTracking();
+    4764             : 
+    4765           0 :   res.success = success;
+    4766           0 :   res.message = message;
+    4767             : 
+    4768           0 :   return true;
+    4769             : }
+    4770             : 
+    4771             : //}
+    4772             : 
+    4773             : /* //{ callbackStopTrajectoryTracking() */
+    4774             : 
+    4775           0 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4776             : 
+    4777           0 :   if (!is_initialized_)
+    4778           0 :     return false;
+    4779             : 
+    4780           0 :   auto [success, message] = stopTrajectoryTracking();
+    4781             : 
+    4782           0 :   res.success = success;
+    4783           0 :   res.message = message;
+    4784             : 
+    4785           0 :   return true;
+    4786             : }
+    4787             : 
+    4788             : //}
+    4789             : 
+    4790             : /* //{ callbackResumeTrajectoryTracking() */
+    4791             : 
+    4792           0 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4793             : 
+    4794           0 :   if (!is_initialized_)
+    4795           0 :     return false;
+    4796             : 
+    4797           0 :   auto [success, message] = resumeTrajectoryTracking();
+    4798             : 
+    4799           0 :   res.success = success;
+    4800           0 :   res.message = message;
+    4801             : 
+    4802           0 :   return true;
+    4803             : }
+    4804             : 
+    4805             : //}
+    4806             : 
+    4807             : /* //{ callbackGotoTrajectoryStart() */
+    4808             : 
+    4809           0 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4810             : 
+    4811           0 :   if (!is_initialized_)
+    4812           0 :     return false;
+    4813             : 
+    4814           0 :   auto [success, message] = gotoTrajectoryStart();
+    4815             : 
+    4816           0 :   res.success = success;
+    4817           0 :   res.message = message;
+    4818             : 
+    4819           0 :   return true;
+    4820             : }
+    4821             : 
+    4822             : //}
+    4823             : 
+    4824             : /* //{ callbackTransformReference() */
+    4825             : 
+    4826           0 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4827             : 
+    4828           0 :   if (!is_initialized_)
+    4829           0 :     return false;
+    4830             : 
+    4831             :   // transform the reference to the current frame
+    4832           0 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4833             : 
+    4834           0 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4835             : 
+    4836           0 :     res.reference = ret.value();
+    4837           0 :     res.message   = "transformation successful";
+    4838           0 :     res.success   = true;
+    4839           0 :     return true;
+    4840             : 
+    4841             :   } else {
+    4842             : 
+    4843           0 :     res.message = "the reference could not be transformed";
+    4844           0 :     res.success = false;
+    4845           0 :     return true;
+    4846             :   }
+    4847             : 
+    4848             :   return true;
+    4849             : }
+    4850             : 
+    4851             : //}
+    4852             : 
+    4853             : /* //{ callbackTransformPose() */
+    4854             : 
+    4855           0 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    4856             : 
+    4857           0 :   if (!is_initialized_)
+    4858           0 :     return false;
+    4859             : 
+    4860             :   // transform the reference to the current frame
+    4861           0 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    4862             : 
+    4863           0 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    4864             : 
+    4865           0 :     res.pose    = ret.value();
+    4866           0 :     res.message = "transformation successful";
+    4867           0 :     res.success = true;
+    4868           0 :     return true;
+    4869             : 
+    4870             :   } else {
+    4871             : 
+    4872           0 :     res.message = "the pose could not be transformed";
+    4873           0 :     res.success = false;
+    4874           0 :     return true;
+    4875             :   }
+    4876             : 
+    4877             :   return true;
+    4878             : }
+    4879             : 
+    4880             : //}
+    4881             : 
+    4882             : /* //{ callbackTransformVector3() */
+    4883             : 
+    4884           0 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    4885             : 
+    4886           0 :   if (!is_initialized_)
+    4887           0 :     return false;
+    4888             : 
+    4889             :   // transform the reference to the current frame
+    4890           0 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    4891             : 
+    4892           0 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    4893             : 
+    4894           0 :     res.vector  = ret.value();
+    4895           0 :     res.message = "transformation successful";
+    4896           0 :     res.success = true;
+    4897           0 :     return true;
+    4898             : 
+    4899             :   } else {
+    4900             : 
+    4901           0 :     res.message = "the twist could not be transformed";
+    4902           0 :     res.success = false;
+    4903           0 :     return true;
+    4904             :   }
+    4905             : 
+    4906             :   return true;
+    4907             : }
+    4908             : 
+    4909             : //}
+    4910             : 
+    4911             : /* //{ callbackEnableBumper() */
+    4912             : 
+    4913           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4914             : 
+    4915           0 :   if (!is_initialized_)
+    4916           0 :     return false;
+    4917             : 
+    4918           0 :   bumper_enabled_ = req.data;
+    4919             : 
+    4920           0 :   std::stringstream ss;
+    4921             : 
+    4922           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    4923             : 
+    4924           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4925             : 
+    4926           0 :   res.success = true;
+    4927           0 :   res.message = ss.str();
+    4928             : 
+    4929           0 :   return true;
+    4930             : }
+    4931             : 
+    4932             : //}
+    4933             : 
+    4934             : /* //{ callbackUseSafetyArea() */
+    4935             : 
+    4936           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4937             : 
+    4938           0 :   if (!is_initialized_)
+    4939           0 :     return false;
+    4940             : 
+    4941           0 :   use_safety_area_ = req.data;
+    4942             : 
+    4943           0 :   std::stringstream ss;
+    4944             : 
+    4945           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    4946             : 
+    4947           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4948             : 
+    4949           0 :   res.success = true;
+    4950           0 :   res.message = ss.str();
+    4951             : 
+    4952           0 :   return true;
+    4953             : }
+    4954             : 
+    4955             : //}
+    4956             : 
+    4957             : /* //{ callbackGetMinZ() */
+    4958             : 
+    4959           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    4960             : 
+    4961           0 :   if (!is_initialized_) {
+    4962           0 :     return false;
+    4963             :   }
+    4964             : 
+    4965           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4966             : 
+    4967           0 :   res.success = true;
+    4968           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    4969             : 
+    4970           0 :   return true;
+    4971             : }
+    4972             : 
+    4973             : //}
+    4974             : 
+    4975             : /* //{ callbackValidateReference() */
+    4976             : 
+    4977           0 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    4978             : 
+    4979           0 :   if (!is_initialized_) {
+    4980           0 :     res.message = "not initialized";
+    4981           0 :     res.success = false;
+    4982           0 :     return true;
+    4983             :   }
+    4984             : 
+    4985           0 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    4986           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    4987           0 :     res.message = "NaNs/infs in input!";
+    4988           0 :     res.success = false;
+    4989           0 :     return true;
+    4990             :   }
+    4991             : 
+    4992             :   // copy member variables
+    4993           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4994           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    4995             : 
+    4996             :   // transform the reference to the current frame
+    4997           0 :   mrs_msgs::ReferenceStamped original_reference;
+    4998           0 :   original_reference.header    = req.reference.header;
+    4999           0 :   original_reference.reference = req.reference.reference;
+    5000             : 
+    5001           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5002             : 
+    5003           0 :   if (!ret) {
+    5004             : 
+    5005           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5006           0 :     res.message = "the reference could not be transformed";
+    5007           0 :     res.success = false;
+    5008           0 :     return true;
+    5009             :   }
+    5010             : 
+    5011           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5012             : 
+    5013           0 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5014           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5015           0 :     res.message = "the point is outside of the safety area";
+    5016           0 :     res.success = false;
+    5017           0 :     return true;
+    5018             :   }
+    5019             : 
+    5020           0 :   if (last_tracker_cmd) {
+    5021             : 
+    5022           0 :     mrs_msgs::ReferenceStamped from_point;
+    5023           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5024           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5025           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5026           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5027             : 
+    5028           0 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5029           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5030           0 :       res.message = "the path is going outside the safety area";
+    5031           0 :       res.success = false;
+    5032           0 :       return true;
+    5033             :     }
+    5034             :   }
+    5035             : 
+    5036           0 :   res.message = "the reference is ok";
+    5037           0 :   res.success = true;
+    5038           0 :   return true;
+    5039             : }
+    5040             : 
+    5041             : //}
+    5042             : 
+    5043             : /* //{ callbackValidateReference2d() */
+    5044             : 
+    5045        1567 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5046             : 
+    5047        1567 :   if (!is_initialized_) {
+    5048           0 :     res.message = "not initialized";
+    5049           0 :     res.success = false;
+    5050           0 :     return true;
+    5051             :   }
+    5052             : 
+    5053        1567 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5054           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5055           0 :     res.message = "NaNs/infs in input!";
+    5056           0 :     res.success = false;
+    5057           0 :     return true;
+    5058             :   }
+    5059             : 
+    5060             :   // copy member variables
+    5061        3134 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5062        3134 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5063             : 
+    5064             :   // transform the reference to the current frame
+    5065        3134 :   mrs_msgs::ReferenceStamped original_reference;
+    5066        1567 :   original_reference.header    = req.reference.header;
+    5067        1567 :   original_reference.reference = req.reference.reference;
+    5068             : 
+    5069        3134 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5070             : 
+    5071        1567 :   if (!ret) {
+    5072             : 
+    5073           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5074           0 :     res.message = "the reference could not be transformed";
+    5075           0 :     res.success = false;
+    5076           0 :     return true;
+    5077             :   }
+    5078             : 
+    5079        3134 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5080             : 
+    5081        1567 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5082          77 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5083          77 :     res.message = "the point is outside of the safety area";
+    5084          77 :     res.success = false;
+    5085          77 :     return true;
+    5086             :   }
+    5087             : 
+    5088        1490 :   if (last_tracker_cmd) {
+    5089             : 
+    5090          31 :     mrs_msgs::ReferenceStamped from_point;
+    5091          31 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5092          31 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5093          31 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5094          31 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5095             : 
+    5096          31 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5097           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5098           0 :       res.message = "the path is going outside the safety area";
+    5099           0 :       res.success = false;
+    5100           0 :       return true;
+    5101             :     }
+    5102             :   }
+    5103             : 
+    5104        1490 :   res.message = "the reference is ok";
+    5105        1490 :   res.success = true;
+    5106        1490 :   return true;
+    5107             : }
+    5108             : 
+    5109             : //}
+    5110             : 
+    5111             : /* //{ callbackValidateReferenceList() */
+    5112             : 
+    5113           0 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+    5114             : 
+    5115           0 :   if (!is_initialized_) {
+    5116           0 :     res.message = "not initialized";
+    5117           0 :     return false;
+    5118             :   }
+    5119             : 
+    5120             :   // copy member variables
+    5121           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5122           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5123             : 
+    5124             :   // get the transformer
+    5125           0 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+    5126             : 
+    5127           0 :   if (!ret) {
+    5128             : 
+    5129           0 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5130           0 :     res.message = "could not find transform";
+    5131           0 :     return false;
+    5132             :   }
+    5133             : 
+    5134           0 :   geometry_msgs::TransformStamped tf = ret.value();
+    5135             : 
+    5136           0 :   for (int i = 0; i < int(req.list.list.size()); i++) {
+    5137             : 
+    5138           0 :     res.success.push_back(true);
+    5139             : 
+    5140           0 :     mrs_msgs::ReferenceStamped original_reference;
+    5141           0 :     original_reference.header    = req.list.header;
+    5142           0 :     original_reference.reference = req.list.list[i];
+    5143             : 
+    5144           0 :     res.success[i] = validateReference(original_reference.reference, "ControlManager", "reference_list");
+    5145             : 
+    5146           0 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5147             : 
+    5148           0 :     if (!ret) {
+    5149             : 
+    5150           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5151           0 :       res.success[i] = false;
+    5152             :     }
+    5153             : 
+    5154           0 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5155             : 
+    5156           0 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5157           0 :       res.success[i] = false;
+    5158             :     }
+    5159             : 
+    5160           0 :     if (last_tracker_cmd) {
+    5161             : 
+    5162           0 :       mrs_msgs::ReferenceStamped from_point;
+    5163           0 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5164           0 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5165           0 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5166           0 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5167             : 
+    5168           0 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5169           0 :         res.success[i] = false;
+    5170             :       }
+    5171             :     }
+    5172             :   }
+    5173             : 
+    5174           0 :   res.message = "references were checked";
+    5175           0 :   return true;
+    5176             : }
+    5177             : 
+    5178             : //}
+    5179             : 
+    5180             : // | -------------- setpoint topics and services -------------- |
+    5181             : 
+    5182             : /* //{ callbackReferenceService() */
+    5183             : 
+    5184           0 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5185             : 
+    5186           0 :   if (!is_initialized_) {
+    5187           0 :     res.message = "not initialized";
+    5188           0 :     res.success = false;
+    5189           0 :     return true;
+    5190             :   }
+    5191             : 
+    5192           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5193           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5194             : 
+    5195           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5196           0 :   des_reference.header    = req.header;
+    5197           0 :   des_reference.reference = req.reference;
+    5198             : 
+    5199           0 :   auto [success, message] = setReference(des_reference);
+    5200             : 
+    5201           0 :   res.success = success;
+    5202           0 :   res.message = message;
+    5203             : 
+    5204           0 :   return true;
+    5205             : }
+    5206             : 
+    5207             : //}
+    5208             : 
+    5209             : /* //{ callbackReferenceTopic() */
+    5210             : 
+    5211           0 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5212             : 
+    5213           0 :   if (!is_initialized_)
+    5214           0 :     return;
+    5215             : 
+    5216           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5217           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5218             : 
+    5219           0 :   setReference(*msg);
+    5220             : }
+    5221             : 
+    5222             : //}
+    5223             : 
+    5224             : /* //{ callbackVelocityReferenceService() */
+    5225             : 
+    5226           0 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5227             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5228             : 
+    5229           0 :   if (!is_initialized_) {
+    5230           0 :     res.message = "not initialized";
+    5231           0 :     res.success = false;
+    5232           0 :     return true;
+    5233             :   }
+    5234             : 
+    5235           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5236           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5237             : 
+    5238           0 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5239           0 :   des_reference = req.reference;
+    5240             : 
+    5241           0 :   auto [success, message] = setVelocityReference(des_reference);
+    5242             : 
+    5243           0 :   res.success = success;
+    5244           0 :   res.message = message;
+    5245             : 
+    5246           0 :   return true;
+    5247             : }
+    5248             : 
+    5249             : //}
+    5250             : 
+    5251             : /* //{ callbackVelocityReferenceTopic() */
+    5252             : 
+    5253           0 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5254             : 
+    5255           0 :   if (!is_initialized_)
+    5256           0 :     return;
+    5257             : 
+    5258           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5259           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5260             : 
+    5261           0 :   setVelocityReference(*msg);
+    5262             : }
+    5263             : 
+    5264             : //}
+    5265             : 
+    5266             : /* //{ callbackTrajectoryReferenceService() */
+    5267             : 
+    5268           1 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5269             : 
+    5270           1 :   if (!is_initialized_) {
+    5271           0 :     res.message = "not initialized";
+    5272           0 :     res.success = false;
+    5273           0 :     return true;
+    5274             :   }
+    5275             : 
+    5276           3 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5277           3 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5278             : 
+    5279           2 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5280             : 
+    5281           1 :   res.success          = success;
+    5282           1 :   res.message          = message;
+    5283           1 :   res.modified         = modified;
+    5284           1 :   res.tracker_names    = tracker_names;
+    5285           1 :   res.tracker_messages = tracker_messages;
+    5286             : 
+    5287           7 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5288           6 :     res.tracker_successes.push_back(tracker_successes[i]);
+    5289             :   }
+    5290             : 
+    5291           1 :   return true;
+    5292             : }
+    5293             : 
+    5294             : //}
+    5295             : 
+    5296             : /* //{ callbackTrajectoryReferenceTopic() */
+    5297             : 
+    5298           0 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5299             : 
+    5300           0 :   if (!is_initialized_)
+    5301           0 :     return;
+    5302             : 
+    5303           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5304           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5305             : 
+    5306           0 :   setTrajectoryReference(*msg);
+    5307             : }
+    5308             : 
+    5309             : //}
+    5310             : 
+    5311             : // | ------------- human-callable "goto" services ------------- |
+    5312             : 
+    5313             : /* //{ callbackGoto() */
+    5314             : 
+    5315          27 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5316             : 
+    5317          27 :   if (!is_initialized_) {
+    5318           0 :     res.message = "not initialized";
+    5319           0 :     res.success = false;
+    5320           0 :     return true;
+    5321             :   }
+    5322             : 
+    5323          81 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5324          81 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5325             : 
+    5326          54 :   mrs_msgs::ReferenceStamped des_reference;
+    5327          27 :   des_reference.header.frame_id      = "";
+    5328          27 :   des_reference.header.stamp         = ros::Time(0);
+    5329          27 :   des_reference.reference.position.x = req.goal[REF_X];
+    5330          27 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5331          27 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5332          27 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5333             : 
+    5334          54 :   auto [success, message] = setReference(des_reference);
+    5335             : 
+    5336          27 :   res.success = success;
+    5337          27 :   res.message = message;
+    5338             : 
+    5339          27 :   return true;
+    5340             : }
+    5341             : 
+    5342             : //}
+    5343             : 
+    5344             : /* //{ callbackGotoFcu() */
+    5345             : 
+    5346           0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5347             : 
+    5348           0 :   if (!is_initialized_) {
+    5349           0 :     res.message = "not initialized";
+    5350           0 :     res.success = false;
+    5351           0 :     return true;
+    5352             :   }
+    5353             : 
+    5354           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5355           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5356             : 
+    5357           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5358           0 :   des_reference.header.frame_id      = "fcu_untilted";
+    5359           0 :   des_reference.header.stamp         = ros::Time(0);
+    5360           0 :   des_reference.reference.position.x = req.goal[REF_X];
+    5361           0 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5362           0 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5363           0 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5364             : 
+    5365           0 :   auto [success, message] = setReference(des_reference);
+    5366             : 
+    5367           0 :   res.success = success;
+    5368           0 :   res.message = message;
+    5369             : 
+    5370           0 :   return true;
+    5371             : }
+    5372             : 
+    5373             : //}
+    5374             : 
+    5375             : /* //{ callbackGotoRelative() */
+    5376             : 
+    5377           3 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5378             : 
+    5379           3 :   if (!is_initialized_) {
+    5380           0 :     res.message = "not initialized";
+    5381           0 :     res.success = false;
+    5382           0 :     return true;
+    5383             :   }
+    5384             : 
+    5385           9 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5386           9 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5387             : 
+    5388           6 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5389             : 
+    5390           3 :   if (!last_tracker_cmd) {
+    5391           0 :     res.message = "not flying";
+    5392           0 :     res.success = false;
+    5393           0 :     return true;
+    5394             :   }
+    5395             : 
+    5396           6 :   mrs_msgs::ReferenceStamped des_reference;
+    5397           3 :   des_reference.header.frame_id      = "";
+    5398           3 :   des_reference.header.stamp         = ros::Time(0);
+    5399           3 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal[REF_X];
+    5400           3 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal[REF_Y];
+    5401           3 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal[REF_Z];
+    5402           3 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal[REF_HEADING];
+    5403             : 
+    5404           6 :   auto [success, message] = setReference(des_reference);
+    5405             : 
+    5406           3 :   res.success = success;
+    5407           3 :   res.message = message;
+    5408             : 
+    5409           3 :   return true;
+    5410             : }
+    5411             : 
+    5412             : //}
+    5413             : 
+    5414             : /* //{ callbackGotoAltitude() */
+    5415             : 
+    5416           0 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5417             : 
+    5418           0 :   if (!is_initialized_) {
+    5419           0 :     res.message = "not initialized";
+    5420           0 :     res.success = false;
+    5421           0 :     return true;
+    5422             :   }
+    5423             : 
+    5424           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5425           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5426             : 
+    5427           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5428             : 
+    5429           0 :   if (!last_tracker_cmd) {
+    5430           0 :     res.message = "not flying";
+    5431           0 :     res.success = false;
+    5432           0 :     return true;
+    5433             :   }
+    5434             : 
+    5435           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5436           0 :   des_reference.header.frame_id      = "";
+    5437           0 :   des_reference.header.stamp         = ros::Time(0);
+    5438           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5439           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5440           0 :   des_reference.reference.position.z = req.goal;
+    5441           0 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5442             : 
+    5443           0 :   auto [success, message] = setReference(des_reference);
+    5444             : 
+    5445           0 :   res.success = success;
+    5446           0 :   res.message = message;
+    5447             : 
+    5448           0 :   return true;
+    5449             : }
+    5450             : 
+    5451             : //}
+    5452             : 
+    5453             : /* //{ callbackSetHeading() */
+    5454             : 
+    5455           0 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5456             : 
+    5457           0 :   if (!is_initialized_) {
+    5458           0 :     res.message = "not initialized";
+    5459           0 :     res.success = false;
+    5460           0 :     return true;
+    5461             :   }
+    5462             : 
+    5463           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5464           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5465             : 
+    5466           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5467             : 
+    5468           0 :   if (!last_tracker_cmd) {
+    5469           0 :     res.message = "not flying";
+    5470           0 :     res.success = false;
+    5471           0 :     return true;
+    5472             :   }
+    5473             : 
+    5474           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5475           0 :   des_reference.header.frame_id      = "";
+    5476           0 :   des_reference.header.stamp         = ros::Time(0);
+    5477           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5478           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5479           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5480           0 :   des_reference.reference.heading    = req.goal;
+    5481             : 
+    5482           0 :   auto [success, message] = setReference(des_reference);
+    5483             : 
+    5484           0 :   res.success = success;
+    5485           0 :   res.message = message;
+    5486             : 
+    5487           0 :   return true;
+    5488             : }
+    5489             : 
+    5490             : //}
+    5491             : 
+    5492             : /* //{ callbackSetHeadingRelative() */
+    5493             : 
+    5494           0 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5495             : 
+    5496           0 :   if (!is_initialized_) {
+    5497           0 :     res.message = "not initialized";
+    5498           0 :     res.success = false;
+    5499           0 :     return true;
+    5500             :   }
+    5501             : 
+    5502           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5503           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5504             : 
+    5505           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5506             : 
+    5507           0 :   if (!last_tracker_cmd) {
+    5508           0 :     res.message = "not flying";
+    5509           0 :     res.success = false;
+    5510           0 :     return true;
+    5511             :   }
+    5512             : 
+    5513           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5514           0 :   des_reference.header.frame_id      = "";
+    5515           0 :   des_reference.header.stamp         = ros::Time(0);
+    5516           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5517           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5518           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5519           0 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5520             : 
+    5521           0 :   auto [success, message] = setReference(des_reference);
+    5522             : 
+    5523           0 :   res.success = success;
+    5524           0 :   res.message = message;
+    5525             : 
+    5526           0 :   return true;
+    5527             : }
+    5528             : 
+    5529             : //}
+    5530             : 
+    5531             : // --------------------------------------------------------------
+    5532             : // |                          routines                          |
+    5533             : // --------------------------------------------------------------
+    5534             : 
+    5535             : /* setReference() //{ */
+    5536             : 
+    5537          30 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5538             : 
+    5539          60 :   std::stringstream ss;
+    5540             : 
+    5541          30 :   if (!callbacks_enabled_) {
+    5542           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5543           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5544           0 :     return std::tuple(false, ss.str());
+    5545             :   }
+    5546             : 
+    5547          30 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5548           0 :     ss << "incoming reference is not finite!!!";
+    5549           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5550           0 :     return std::tuple(false, ss.str());
+    5551             :   }
+    5552             : 
+    5553             :   // copy member variables
+    5554          60 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5555          60 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5556             : 
+    5557             :   // transform the reference to the current frame
+    5558          60 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5559             : 
+    5560          30 :   if (!ret) {
+    5561             : 
+    5562           0 :     ss << "the reference could not be transformed";
+    5563           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5564           0 :     return std::tuple(false, ss.str());
+    5565             :   }
+    5566             : 
+    5567          60 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5568             : 
+    5569             :   // safety area check
+    5570          30 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5571           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5572           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5573           0 :     return std::tuple(false, ss.str());
+    5574             :   }
+    5575             : 
+    5576          30 :   if (last_tracker_cmd) {
+    5577             : 
+    5578          30 :     mrs_msgs::ReferenceStamped from_point;
+    5579          30 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5580          30 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5581          30 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5582          30 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5583             : 
+    5584          30 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5585           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5586           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5587           0 :       return std::tuple(false, ss.str());
+    5588             :     }
+    5589             :   }
+    5590             : 
+    5591          30 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5592             : 
+    5593             :   // prepare the message for current tracker
+    5594          30 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5595          30 :   reference_request.reference = transformed_reference.reference;
+    5596             : 
+    5597             :   {
+    5598          60 :     std::scoped_lock lock(mutex_tracker_list_);
+    5599             : 
+    5600          90 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    5601          90 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5602             : 
+    5603          30 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5604             : 
+    5605          60 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5606             : 
+    5607             :     } else {
+    5608             : 
+    5609           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    5610           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5611           0 :       return std::tuple(false, ss.str());
+    5612             :     }
+    5613             :   }
+    5614             : }
+    5615             : 
+    5616             : //}
+    5617             : 
+    5618             : /* setVelocityReference() //{ */
+    5619             : 
+    5620           0 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5621             : 
+    5622           0 :   std::stringstream ss;
+    5623             : 
+    5624           0 :   if (!callbacks_enabled_) {
+    5625           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5626           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5627           0 :     return std::tuple(false, ss.str());
+    5628             :   }
+    5629             : 
+    5630           0 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5631           0 :     ss << "velocity command is not valid!";
+    5632           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5633           0 :     return std::tuple(false, ss.str());
+    5634             :   }
+    5635             : 
+    5636             :   {
+    5637           0 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5638             : 
+    5639           0 :     if (!last_tracker_cmd_) {
+    5640           0 :       ss << "could not set velocity command, not flying!";
+    5641           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5642           0 :       return std::tuple(false, ss.str());
+    5643             :     }
+    5644             :   }
+    5645             : 
+    5646             :   // copy member variables
+    5647           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5648           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5649             : 
+    5650             :   // | -- transform the velocity reference to the current frame - |
+    5651             : 
+    5652           0 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5653             : 
+    5654           0 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5655             : 
+    5656           0 :   geometry_msgs::TransformStamped tf;
+    5657             : 
+    5658           0 :   if (!ret) {
+    5659           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5660           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5661           0 :     return std::tuple(false, ss.str());
+    5662             :   } else {
+    5663           0 :     tf = ret.value();
+    5664             :   }
+    5665             : 
+    5666             :   // transform the velocity
+    5667             :   {
+    5668           0 :     geometry_msgs::Vector3Stamped velocity;
+    5669           0 :     velocity.header   = reference_in.header;
+    5670           0 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5671           0 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5672           0 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5673             : 
+    5674           0 :     auto ret = transformer_->transform(velocity, tf);
+    5675             : 
+    5676           0 :     if (!ret) {
+    5677             : 
+    5678           0 :       ss << "the velocity reference could not be transformed";
+    5679           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5680           0 :       return std::tuple(false, ss.str());
+    5681             : 
+    5682             :     } else {
+    5683           0 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5684           0 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5685           0 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5686             :     }
+    5687             :   }
+    5688             : 
+    5689             :   // transform the z and the heading
+    5690             :   {
+    5691           0 :     geometry_msgs::PoseStamped pose;
+    5692           0 :     pose.header           = reference_in.header;
+    5693           0 :     pose.pose.position.x  = 0;
+    5694           0 :     pose.pose.position.y  = 0;
+    5695           0 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5696           0 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5697             : 
+    5698           0 :     auto ret = transformer_->transform(pose, tf);
+    5699             : 
+    5700           0 :     if (!ret) {
+    5701             : 
+    5702           0 :       ss << "the velocity reference could not be transformed";
+    5703           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5704           0 :       return std::tuple(false, ss.str());
+    5705             : 
+    5706             :     } else {
+    5707           0 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5708           0 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5709             :     }
+    5710             :   }
+    5711             : 
+    5712             :   // the heading rate doees not need to be transformed
+    5713           0 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5714             : 
+    5715           0 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5716           0 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5717             : 
+    5718           0 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5719             : 
+    5720           0 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5721             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5722             : 
+    5723             :   // safety area check
+    5724           0 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5725           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5726           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5727           0 :     return std::tuple(false, ss.str());
+    5728             :   }
+    5729             : 
+    5730           0 :   if (last_tracker_cmd) {
+    5731             : 
+    5732           0 :     mrs_msgs::ReferenceStamped from_point;
+    5733           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5734           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5735           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5736           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5737             : 
+    5738           0 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5739           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5740           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5741           0 :       return std::tuple(false, ss.str());
+    5742             :     }
+    5743             :   }
+    5744             : 
+    5745           0 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5746             : 
+    5747             :   // prepare the message for current tracker
+    5748           0 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5749           0 :   reference_request.reference = transformed_reference.reference;
+    5750             : 
+    5751             :   {
+    5752           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    5753             : 
+    5754           0 :     tracker_response = tracker_list_[active_tracker_idx_]->setVelocityReference(
+    5755           0 :         mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5756             : 
+    5757           0 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5758             : 
+    5759           0 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5760             : 
+    5761             :     } else {
+    5762             : 
+    5763           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setVelocityReference()' function!";
+    5764           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5765           0 :       return std::tuple(false, ss.str());
+    5766             :     }
+    5767             :   }
+    5768             : }
+    5769             : 
+    5770             : //}
+    5771             : 
+    5772             : /* setTrajectoryReference() //{ */
+    5773             : 
+    5774           1 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5775             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5776             : 
+    5777           2 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5778           2 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5779             : 
+    5780           2 :   std::stringstream ss;
+    5781             : 
+    5782           1 :   if (!callbacks_enabled_) {
+    5783           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5784           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5785           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5786             :   }
+    5787             : 
+    5788             :   /* validate the size and check for NaNs //{ */
+    5789             : 
+    5790             :   // check for the size 0, which is invalid
+    5791           1 :   if (trajectory_in.points.size() == 0) {
+    5792             : 
+    5793           0 :     ss << "can not load trajectory with size 0";
+    5794           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5795           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5796             :   }
+    5797             : 
+    5798          41 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5799             : 
+    5800             :     // check the point for NaN/inf
+    5801          40 :     bool valid = validateReference(trajectory_in.points[i], "ControlManager", "trajectory_in.points[]");
+    5802             : 
+    5803          40 :     if (!valid) {
+    5804             : 
+    5805           0 :       ss << "trajectory contains NaNs/infs.";
+    5806           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5807           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5808             :     }
+    5809             :   }
+    5810             : 
+    5811             :   //}
+    5812             : 
+    5813             :   /* publish the debugging topics of the original trajectory //{ */
+    5814             : 
+    5815             :   {
+    5816             : 
+    5817           2 :     geometry_msgs::PoseArray debug_trajectory_out;
+    5818           1 :     debug_trajectory_out.header = trajectory_in.header;
+    5819             : 
+    5820           1 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    5821             : 
+    5822           1 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    5823           1 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    5824             :     }
+    5825             : 
+    5826          40 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5827             : 
+    5828          39 :       geometry_msgs::Pose new_pose;
+    5829             : 
+    5830          39 :       new_pose.position.x = trajectory_in.points[i].position.x;
+    5831          39 :       new_pose.position.y = trajectory_in.points[i].position.y;
+    5832          39 :       new_pose.position.z = trajectory_in.points[i].position.z;
+    5833             : 
+    5834          39 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points[i].heading);
+    5835             : 
+    5836          39 :       debug_trajectory_out.poses.push_back(new_pose);
+    5837             :     }
+    5838             : 
+    5839           1 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    5840             : 
+    5841           2 :     visualization_msgs::MarkerArray msg_out;
+    5842             : 
+    5843           2 :     visualization_msgs::Marker marker;
+    5844             : 
+    5845           1 :     marker.header = trajectory_in.header;
+    5846             : 
+    5847           1 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    5848             : 
+    5849           1 :     if (marker.header.frame_id == "") {
+    5850           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    5851             :     }
+    5852             : 
+    5853           1 :     if (marker.header.stamp == ros::Time(0)) {
+    5854           1 :       marker.header.stamp = ros::Time::now();
+    5855             :     }
+    5856             : 
+    5857           1 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    5858           1 :     marker.color.a          = 1;
+    5859           1 :     marker.scale.x          = 0.05;
+    5860           1 :     marker.color.r          = 0;
+    5861           1 :     marker.color.g          = 1;
+    5862           1 :     marker.color.b          = 0;
+    5863           1 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    5864             : 
+    5865          40 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5866             : 
+    5867          39 :       geometry_msgs::Point point1;
+    5868             : 
+    5869          39 :       point1.x = trajectory_in.points[i].position.x;
+    5870          39 :       point1.y = trajectory_in.points[i].position.y;
+    5871          39 :       point1.z = trajectory_in.points[i].position.z;
+    5872             : 
+    5873          39 :       marker.points.push_back(point1);
+    5874             : 
+    5875          39 :       geometry_msgs::Point point2;
+    5876             : 
+    5877          39 :       point2.x = trajectory_in.points[i + 1].position.x;
+    5878          39 :       point2.y = trajectory_in.points[i + 1].position.y;
+    5879          39 :       point2.z = trajectory_in.points[i + 1].position.z;
+    5880             : 
+    5881          39 :       marker.points.push_back(point2);
+    5882             :     }
+    5883             : 
+    5884           1 :     msg_out.markers.push_back(marker);
+    5885             : 
+    5886           1 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    5887             :   }
+    5888             : 
+    5889             :   //}
+    5890             : 
+    5891           2 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    5892             : 
+    5893           1 :   int trajectory_size = int(processed_trajectory.points.size());
+    5894             : 
+    5895           1 :   bool trajectory_modified = false;
+    5896             : 
+    5897             :   /* safety area check //{ */
+    5898             : 
+    5899           1 :   if (use_safety_area_) {
+    5900             : 
+    5901           1 :     int last_valid_idx    = 0;
+    5902           1 :     int first_invalid_idx = -1;
+    5903             : 
+    5904           1 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    5905           1 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    5906             : 
+    5907          41 :     for (int i = 0; i < trajectory_size; i++) {
+    5908             : 
+    5909          40 :       if (_snap_trajectory_to_safety_area_) {
+    5910             : 
+    5911             :         // saturate the trajectory to min and max Z
+    5912           0 :         if (processed_trajectory.points[i].position.z < min_z) {
+    5913             : 
+    5914           0 :           processed_trajectory.points[i].position.z = min_z;
+    5915           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    5916           0 :           trajectory_modified = true;
+    5917             :         }
+    5918             : 
+    5919           0 :         if (processed_trajectory.points[i].position.z > max_z) {
+    5920             : 
+    5921           0 :           processed_trajectory.points[i].position.z = max_z;
+    5922           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    5923           0 :           trajectory_modified = true;
+    5924             :         }
+    5925             :       }
+    5926             : 
+    5927             :       // check the point against the safety area
+    5928          40 :       mrs_msgs::ReferenceStamped des_reference;
+    5929          40 :       des_reference.header    = processed_trajectory.header;
+    5930          40 :       des_reference.reference = processed_trajectory.points[i];
+    5931             : 
+    5932          40 :       if (!isPointInSafetyArea3d(des_reference)) {
+    5933             : 
+    5934           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    5935           0 :         trajectory_modified = true;
+    5936             : 
+    5937             :         // the first invalid point
+    5938           0 :         if (first_invalid_idx == -1) {
+    5939             : 
+    5940           0 :           first_invalid_idx = i;
+    5941             : 
+    5942           0 :           last_valid_idx = i - 1;
+    5943             :         }
+    5944             : 
+    5945             :         // the point is ok
+    5946             :       } else {
+    5947             : 
+    5948             :         // we found a point, which is ok, after finding a point which was not ok
+    5949          40 :         if (first_invalid_idx != -1) {
+    5950             : 
+    5951             :           // special case, we had no valid point so far
+    5952           0 :           if (last_valid_idx == -1) {
+    5953             : 
+    5954           0 :             ss << "the trajectory starts outside of the safety area!";
+    5955           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5956           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5957             : 
+    5958             :             // we have a valid point in the past
+    5959             :           } else {
+    5960             : 
+    5961           0 :             if (!_snap_trajectory_to_safety_area_) {
+    5962           0 :               break;
+    5963             :             }
+    5964             : 
+    5965           0 :             bool interpolation_success = true;
+    5966             : 
+    5967             :             // iterpolate between the last valid point and this new valid point
+    5968           0 :             double angle = atan2((processed_trajectory.points[i].position.y - processed_trajectory.points[last_valid_idx].position.y),
+    5969           0 :                                  (processed_trajectory.points[i].position.x - processed_trajectory.points[last_valid_idx].position.x));
+    5970             : 
+    5971             :             double dist_two_points =
+    5972           0 :                 mrs_lib::geometry::dist(vec2_t(processed_trajectory.points[i].position.x, processed_trajectory.points[i].position.y),
+    5973           0 :                                         vec2_t(processed_trajectory.points[last_valid_idx].position.x, processed_trajectory.points[last_valid_idx].position.y));
+    5974           0 :             double step = dist_two_points / (i - last_valid_idx);
+    5975             : 
+    5976           0 :             for (int j = last_valid_idx; j < i; j++) {
+    5977             : 
+    5978           0 :               mrs_msgs::ReferenceStamped temp_point;
+    5979           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    5980           0 :               temp_point.reference.position.x = processed_trajectory.points[last_valid_idx].position.x + (j - last_valid_idx) * cos(angle) * step;
+    5981           0 :               temp_point.reference.position.y = processed_trajectory.points[last_valid_idx].position.y + (j - last_valid_idx) * sin(angle) * step;
+    5982             : 
+    5983           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    5984             : 
+    5985           0 :                 interpolation_success = false;
+    5986           0 :                 break;
+    5987             : 
+    5988             :               } else {
+    5989             : 
+    5990           0 :                 processed_trajectory.points[j].position.x = temp_point.reference.position.x;
+    5991           0 :                 processed_trajectory.points[j].position.y = temp_point.reference.position.y;
+    5992             :               }
+    5993             :             }
+    5994             : 
+    5995           0 :             if (!interpolation_success) {
+    5996           0 :               break;
+    5997             :             }
+    5998             :           }
+    5999             : 
+    6000           0 :           first_invalid_idx = -1;
+    6001             :         }
+    6002             :       }
+    6003             :     }
+    6004             : 
+    6005             :     // special case, the trajectory does not end with a valid point
+    6006           1 :     if (first_invalid_idx != -1) {
+    6007             : 
+    6008             :       // super special case, the whole trajectory is invalid
+    6009           0 :       if (first_invalid_idx == 0) {
+    6010             : 
+    6011           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6012           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6013           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6014             : 
+    6015             :         // there is a good portion of the trajectory in the beginning
+    6016             :       } else {
+    6017             : 
+    6018           0 :         trajectory_size = last_valid_idx + 1;
+    6019           0 :         processed_trajectory.points.resize(trajectory_size);
+    6020           0 :         trajectory_modified = true;
+    6021             :       }
+    6022             :     }
+    6023             :   }
+    6024             : 
+    6025           1 :   if (trajectory_size == 0) {
+    6026             : 
+    6027           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6028           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6029           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6030             :   }
+    6031             : 
+    6032             :   //}
+    6033             : 
+    6034             :   /* transform the trajectory to the current control frame //{ */
+    6035             : 
+    6036           1 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6037             : 
+    6038           1 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6039           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6040             :   } else {
+    6041           1 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6042             :   }
+    6043             : 
+    6044           1 :   if (!tf_traj_state) {
+    6045           0 :     ss << "could not create TF transformer for the trajectory";
+    6046           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6047           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6048             :   }
+    6049             : 
+    6050           1 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6051             : 
+    6052          41 :   for (int i = 0; i < trajectory_size; i++) {
+    6053             : 
+    6054          40 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6055          40 :     trajectory_point.header    = processed_trajectory.header;
+    6056          40 :     trajectory_point.reference = processed_trajectory.points[i];
+    6057             : 
+    6058          40 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6059             : 
+    6060          40 :     if (!ret) {
+    6061             : 
+    6062           0 :       ss << "trajectory cannnot be transformed";
+    6063           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6064           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6065             : 
+    6066             :     } else {
+    6067             : 
+    6068             :       // transform the points in the trajectory to the current frame
+    6069          40 :       processed_trajectory.points[i] = ret.value().reference;
+    6070             :     }
+    6071             :   }
+    6072             : 
+    6073             :   //}
+    6074             : 
+    6075           1 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6076           2 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6077             : 
+    6078             :   // check for empty trajectory
+    6079           1 :   if (processed_trajectory.points.size() == 0) {
+    6080           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6081           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6082           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6083             :   }
+    6084             : 
+    6085             :   // prepare the message for current tracker
+    6086           1 :   request.trajectory = processed_trajectory;
+    6087             : 
+    6088             :   bool                     success;
+    6089           2 :   std::string              message;
+    6090             :   bool                     modified;
+    6091           2 :   std::vector<std::string> tracker_names;
+    6092           2 :   std::vector<bool>        tracker_successes;
+    6093           2 :   std::vector<std::string> tracker_messages;
+    6094             : 
+    6095             :   {
+    6096           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    6097             : 
+    6098             :     // set the trajectory to the currently active tracker
+    6099           3 :     response = tracker_list_[active_tracker_idx_]->setTrajectoryReference(
+    6100           3 :         mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6101             : 
+    6102           1 :     tracker_names.push_back(_tracker_names_[active_tracker_idx_]);
+    6103             : 
+    6104           1 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6105             : 
+    6106           1 :       success  = response->success;
+    6107           1 :       message  = response->message;
+    6108           1 :       modified = response->modified || trajectory_modified;
+    6109           1 :       tracker_successes.push_back(response->success);
+    6110           1 :       tracker_messages.push_back(response->message);
+    6111             : 
+    6112             :     } else {
+    6113             : 
+    6114           0 :       ss << "the active tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setTrajectoryReference()' function!";
+    6115           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the trajectory: " << ss.str());
+    6116             : 
+    6117           0 :       success  = false;
+    6118           0 :       message  = ss.str();
+    6119           0 :       modified = false;
+    6120           0 :       tracker_successes.push_back(false);
+    6121           0 :       tracker_messages.push_back(ss.str());
+    6122             :     }
+    6123             : 
+    6124             :     // set the trajectory to the non-active trackers
+    6125           7 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6126             : 
+    6127           6 :       if (i != active_tracker_idx_) {
+    6128             : 
+    6129           5 :         tracker_names.push_back(_tracker_names_[i]);
+    6130             : 
+    6131          15 :         response = tracker_list_[i]->setTrajectoryReference(
+    6132          15 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6133             : 
+    6134           5 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6135             : 
+    6136           0 :           tracker_successes.push_back(response->success);
+    6137           0 :           tracker_messages.push_back(response->message);
+    6138             : 
+    6139           0 :           if (response->success) {
+    6140           0 :             std::stringstream ss;
+    6141           0 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_[i];
+    6142           0 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6143             :           }
+    6144             : 
+    6145             :         } else {
+    6146             : 
+    6147           5 :           std::stringstream ss;
+    6148           5 :           ss << "the tracker \"" << _tracker_names_[i] << "\" does not implement setTrajectoryReference()";
+    6149           5 :           tracker_successes.push_back(false);
+    6150           5 :           tracker_messages.push_back(ss.str());
+    6151             :         }
+    6152             :       }
+    6153             :     }
+    6154             :   }
+    6155             : 
+    6156           1 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6157             : }
+    6158             : 
+    6159             : //}
+    6160             : 
+    6161             : /* isOffboard() //{ */
+    6162             : 
+    6163           6 : bool ControlManager::isOffboard(void) {
+    6164             : 
+    6165           6 :   if (!sh_hw_api_status_.hasMsg()) {
+    6166           0 :     return false;
+    6167             :   }
+    6168             : 
+    6169           6 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6170             : 
+    6171           6 :   return hw_api_status->connected && hw_api_status->offboard;
+    6172             : }
+    6173             : 
+    6174             : //}
+    6175             : 
+    6176             : /* setCallbacks() //{ */
+    6177             : 
+    6178           2 : void ControlManager::setCallbacks(bool in) {
+    6179             : 
+    6180           2 :   callbacks_enabled_ = in;
+    6181             : 
+    6182           2 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6183           2 :   req_enable_callbacks.data = callbacks_enabled_;
+    6184             : 
+    6185             :   {
+    6186           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    6187             : 
+    6188             :     // set callbacks to all trackers
+    6189          14 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6190          12 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6191             :     }
+    6192             :   }
+    6193           2 : }
+    6194             : 
+    6195             : //}
+    6196             : 
+    6197             : /* publishDiagnostics() //{ */
+    6198             : 
+    6199        4901 : void ControlManager::publishDiagnostics(void) {
+    6200             : 
+    6201        4901 :   if (!is_initialized_) {
+    6202           0 :     return;
+    6203             :   }
+    6204             : 
+    6205       14703 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6206       14703 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6207             : 
+    6208        9802 :   std::scoped_lock lock(mutex_diagnostics_);
+    6209             : 
+    6210        9802 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6211             : 
+    6212        4901 :   diagnostics_msg.stamp    = ros::Time::now();
+    6213        4901 :   diagnostics_msg.uav_name = _uav_name_;
+    6214             : 
+    6215        4901 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6216             : 
+    6217        4901 :   diagnostics_msg.output_enabled = output_enabled_;
+    6218             : 
+    6219        4901 :   diagnostics_msg.rc_mode = rc_goto_active_;
+    6220             : 
+    6221             :   {
+    6222        4901 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6223             : 
+    6224        4901 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6225             :   }
+    6226             : 
+    6227             :   // | ----------------- fill the tracker status ---------------- |
+    6228             : 
+    6229             :   {
+    6230        9802 :     std::scoped_lock lock(mutex_tracker_list_);
+    6231             : 
+    6232        4901 :     mrs_msgs::TrackerStatus tracker_status;
+    6233             : 
+    6234        4901 :     diagnostics_msg.active_tracker = _tracker_names_[active_tracker_idx_];
+    6235        4901 :     diagnostics_msg.tracker_status = tracker_list_[active_tracker_idx_]->getStatus();
+    6236             :   }
+    6237             : 
+    6238             :   // | --------------- fill the controller status --------------- |
+    6239             : 
+    6240             :   {
+    6241        9802 :     std::scoped_lock lock(mutex_controller_list_);
+    6242             : 
+    6243        4901 :     mrs_msgs::ControllerStatus controller_status;
+    6244             : 
+    6245        4901 :     diagnostics_msg.active_controller = _controller_names_[active_controller_idx_];
+    6246        4901 :     diagnostics_msg.controller_status = controller_list_[active_controller_idx_]->getStatus();
+    6247             :   }
+    6248             : 
+    6249             :   // | ------------ fill in the available controllers ----------- |
+    6250             : 
+    6251       29406 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6252       24505 :     if ((_controller_names_[i] != _failsafe_controller_name_) && (_controller_names_[i] != _eland_controller_name_)) {
+    6253       14703 :       diagnostics_msg.available_controllers.push_back(_controller_names_[i]);
+    6254       14703 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_[i]).human_switchable);
+    6255             :     }
+    6256             :   }
+    6257             : 
+    6258             :   // | ------------- fill in the available trackers ------------- |
+    6259             : 
+    6260       34566 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6261       29665 :     if (_tracker_names_[i] != _null_tracker_name_) {
+    6262       24764 :       diagnostics_msg.available_trackers.push_back(_tracker_names_[i]);
+    6263       24764 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_[i]).human_switchable);
+    6264             :     }
+    6265             :   }
+    6266             : 
+    6267             :   // | ------------------------- publish ------------------------ |
+    6268             : 
+    6269        4901 :   ph_diagnostics_.publish(diagnostics_msg);
+    6270             : }
+    6271             : 
+    6272             : //}
+    6273             : 
+    6274             : /* setConstraintsToTrackers() //{ */
+    6275             : 
+    6276         123 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6277             : 
+    6278         369 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6279         369 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6280             : 
+    6281         123 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6282             : 
+    6283             :   {
+    6284         246 :     std::scoped_lock lock(mutex_tracker_list_);
+    6285             : 
+    6286             :     // for each tracker
+    6287         864 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6288             : 
+    6289             :       // if it is the active one, update and retrieve the command
+    6290        2223 :       response = tracker_list_[i]->setConstraints(
+    6291        2223 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6292             :     }
+    6293             :   }
+    6294         123 : }
+    6295             : 
+    6296             : //}
+    6297             : 
+    6298             : /* setConstraintsToControllers() //{ */
+    6299             : 
+    6300         162 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6301             : 
+    6302         486 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6303         486 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6304             : 
+    6305         162 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6306             : 
+    6307             :   {
+    6308         324 :     std::scoped_lock lock(mutex_controller_list_);
+    6309             : 
+    6310             :     // for each controller
+    6311         972 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6312             : 
+    6313             :       // if it is the active one, update and retrieve the command
+    6314        2430 :       response = controller_list_[i]->setConstraints(
+    6315        2430 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6316             :     }
+    6317             :   }
+    6318         162 : }
+    6319             : 
+    6320             : //}
+    6321             : 
+    6322             : /* setConstraints() //{ */
+    6323             : 
+    6324          42 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6325             : 
+    6326         126 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6327         126 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6328             : 
+    6329          42 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6330             : 
+    6331          42 :   setConstraintsToTrackers(constraints);
+    6332             : 
+    6333          42 :   setConstraintsToControllers(constraints);
+    6334          42 : }
+    6335             : 
+    6336             : //}
+    6337             : 
+    6338             : 
+    6339             : /* enforceControllerConstraints() //{ */
+    6340             : 
+    6341       39041 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6342             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6343             : 
+    6344             :   // copy member variables
+    6345       78082 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6346             : 
+    6347       39041 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6348       30399 :     return {};
+    6349             :   }
+    6350             : 
+    6351        8642 :   bool enforcing = false;
+    6352             : 
+    6353        8642 :   auto constraints_out = constraints;
+    6354             : 
+    6355       17284 :   std::scoped_lock lock(mutex_tracker_list_);
+    6356             : 
+    6357             :   // enforce horizontal speed
+    6358        8642 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6359        6394 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6360             : 
+    6361        6394 :     enforcing = true;
+    6362             :   }
+    6363             : 
+    6364             :   // enforce horizontal acceleration
+    6365        8642 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6366        7052 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6367             : 
+    6368        7052 :     enforcing = true;
+    6369             :   }
+    6370             : 
+    6371             :   // enforce vertical ascending speed
+    6372        8642 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6373        6394 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6374             : 
+    6375        6394 :     enforcing = true;
+    6376             :   }
+    6377             : 
+    6378             :   // enforce vertical ascending acceleration
+    6379        8642 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6380           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6381             : 
+    6382           0 :     enforcing = true;
+    6383             :   }
+    6384             : 
+    6385             :   // enforce vertical descending speed
+    6386        8642 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6387        6394 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6388             : 
+    6389        6394 :     enforcing = true;
+    6390             :   }
+    6391             : 
+    6392             :   // enforce vertical descending acceleration
+    6393        8642 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6394           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6395             : 
+    6396           0 :     enforcing = true;
+    6397             :   }
+    6398             : 
+    6399        8642 :   if (enforcing) {
+    6400        7052 :     return {constraints_out};
+    6401             :   } else {
+    6402        1590 :     return {};
+    6403             :   }
+    6404             : }
+    6405             : 
+    6406             : //}
+    6407             : 
+    6408             : /* isFlyingNormally() //{ */
+    6409             : 
+    6410        4901 : bool ControlManager::isFlyingNormally(void) {
+    6411             : 
+    6412        8323 :   return (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6413        3286 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6414        3426 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6415        9470 :           _controller_names_.size() == 1) &&
+    6416        7040 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6417             : }
+    6418             : 
+    6419             : //}
+    6420             : 
+    6421             : /* //{ getMass() */
+    6422             : 
+    6423         214 : double ControlManager::getMass(void) {
+    6424             : 
+    6425         428 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6426             : 
+    6427         214 :   if (last_control_output.diagnostics.mass_estimator) {
+    6428           4 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6429             :   } else {
+    6430         210 :     return _uav_mass_;
+    6431             :   }
+    6432             : }
+    6433             : 
+    6434             : //}
+    6435             : 
+    6436             : /* loadConfigFile() //{ */
+    6437             : 
+    6438           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6439             : 
+    6440           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6441             : 
+    6442           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6443             : 
+    6444             :   // load the user-requested file
+    6445             :   {
+    6446           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6447           0 :     int         result  = std::system(command.c_str());
+    6448             : 
+    6449           0 :     if (result != 0) {
+    6450           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6451           0 :       return false;
+    6452             :     }
+    6453             :   }
+    6454             : 
+    6455             :   // load the platform config
+    6456           0 :   if (_platform_config_ != "") {
+    6457           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6458           0 :     int         result  = std::system(command.c_str());
+    6459             : 
+    6460           0 :     if (result != 0) {
+    6461           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6462           0 :       return false;
+    6463             :     }
+    6464             :   }
+    6465             : 
+    6466             :   // load the custom config
+    6467           0 :   if (_custom_config_ != "") {
+    6468           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6469           0 :     int         result  = std::system(command.c_str());
+    6470             : 
+    6471           0 :     if (result != 0) {
+    6472           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6473           0 :       return false;
+    6474             :     }
+    6475             :   }
+    6476             : 
+    6477           0 :   return true;
+    6478             : }
+    6479             : 
+    6480             : //}
+    6481             : 
+    6482             : // | ----------------------- safety area ---------------------- |
+    6483             : 
+    6484             : /* //{ isPointInSafetyArea3d() */
+    6485             : 
+    6486         126 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6487             : 
+    6488         126 :   if (!use_safety_area_) {
+    6489           2 :     return true;
+    6490             :   }
+    6491             : 
+    6492         248 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6493             : 
+    6494         124 :   if (!tfed_horizontal) {
+    6495           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6496           0 :     return false;
+    6497             :   }
+    6498             : 
+    6499         124 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6500           0 :     return false;
+    6501             :   }
+    6502             : 
+    6503         124 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6504           0 :     return false;
+    6505             :   }
+    6506             : 
+    6507         124 :   return true;
+    6508             : }
+    6509             : 
+    6510             : //}
+    6511             : 
+    6512             : /* //{ isPointInSafetyArea2d() */
+    6513             : 
+    6514        1668 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6515             : 
+    6516        1668 :   if (!use_safety_area_) {
+    6517           3 :     return true;
+    6518             :   }
+    6519             : 
+    6520        3330 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6521             : 
+    6522        1665 :   if (!tfed_horizontal) {
+    6523           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6524           0 :     return false;
+    6525             :   }
+    6526             : 
+    6527        1665 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6528          77 :     return false;
+    6529             :   }
+    6530             : 
+    6531        1588 :   return true;
+    6532             : }
+    6533             : 
+    6534             : //}
+    6535             : 
+    6536             : /* //{ isPathToPointInSafetyArea3d() */
+    6537             : 
+    6538          30 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6539             : 
+    6540          30 :   if (!use_safety_area_) {
+    6541           2 :     return true;
+    6542             :   }
+    6543             : 
+    6544          28 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6545           0 :     return false;
+    6546             :   }
+    6547             : 
+    6548          56 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6549             : 
+    6550             :   {
+    6551          28 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6552             : 
+    6553          28 :     if (!ret) {
+    6554             : 
+    6555           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6556             : 
+    6557           0 :       return false;
+    6558             :     }
+    6559             : 
+    6560          28 :     start_transformed = ret.value();
+    6561             :   }
+    6562             : 
+    6563             :   {
+    6564          28 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6565             : 
+    6566          28 :     if (!ret) {
+    6567             : 
+    6568           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6569             : 
+    6570           0 :       return false;
+    6571             :     }
+    6572             : 
+    6573          28 :     end_transformed = ret.value();
+    6574             :   }
+    6575             : 
+    6576          28 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6577          28 :                                    end_transformed.reference.position.y);
+    6578             : }
+    6579             : 
+    6580             : //}
+    6581             : 
+    6582             : /* //{ isPathToPointInSafetyArea2d() */
+    6583             : 
+    6584          31 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6585          31 :   if (!use_safety_area_) {
+    6586           0 :     return true;
+    6587             :   }
+    6588             : 
+    6589          62 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6590             : 
+    6591          31 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6592           0 :     return false;
+    6593             :   }
+    6594             : 
+    6595             :   {
+    6596          31 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6597             : 
+    6598          31 :     if (!ret) {
+    6599             : 
+    6600           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6601             : 
+    6602           0 :       return false;
+    6603             :     }
+    6604             : 
+    6605          31 :     start_transformed = ret.value();
+    6606             :   }
+    6607             : 
+    6608             :   {
+    6609          31 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6610             : 
+    6611          31 :     if (!ret) {
+    6612             : 
+    6613           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6614             : 
+    6615           0 :       return false;
+    6616             :     }
+    6617             : 
+    6618          31 :     end_transformed = ret.value();
+    6619             :   }
+    6620             : 
+    6621          31 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6622          31 :                                    end_transformed.reference.position.y);
+    6623             : }
+    6624             : 
+    6625             : //}
+    6626             : 
+    6627             : /* //{ getMaxZ() */
+    6628             : 
+    6629        3580 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6630             : 
+    6631             :   // | ------- first, calculate max_z from the safety area ------ |
+    6632             : 
+    6633        3580 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6634             : 
+    6635             :   {
+    6636             : 
+    6637        7160 :     geometry_msgs::PointStamped point;
+    6638             : 
+    6639        3580 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6640        3580 :     point.point.x         = 0;
+    6641        3580 :     point.point.y         = 0;
+    6642        3580 :     point.point.z         = _safety_area_max_z_;
+    6643             : 
+    6644        3580 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6645             : 
+    6646        3580 :     if (!ret) {
+    6647           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6648             :     }
+    6649             : 
+    6650        3580 :     safety_area_max_z = ret->point.z;
+    6651             :   }
+    6652             : 
+    6653             :   // | ------------ overwrite from estimation manager ----------- |
+    6654             : 
+    6655        3580 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6656             : 
+    6657             :   {
+    6658             :     // if possible, override it with max z from the estimation manager
+    6659        3580 :     if (sh_max_z_.hasMsg()) {
+    6660             : 
+    6661        7150 :       auto msg = sh_max_z_.getMsg();
+    6662             : 
+    6663             :       // transform it into the safety area frame
+    6664        7150 :       geometry_msgs::PointStamped point;
+    6665        3575 :       point.header  = msg->header;
+    6666        3575 :       point.point.x = 0;
+    6667        3575 :       point.point.y = 0;
+    6668        3575 :       point.point.z = msg->value;
+    6669             : 
+    6670        3575 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6671             : 
+    6672        3575 :       if (!ret) {
+    6673           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6674             :       }
+    6675             : 
+    6676        3575 :       estimation_manager_max_z = ret->point.z;
+    6677             :     }
+    6678             :   }
+    6679             : 
+    6680        3580 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6681        1119 :     return estimation_manager_max_z;
+    6682             :   } else {
+    6683        2461 :     return safety_area_max_z;
+    6684             :   }
+    6685             : }
+    6686             : 
+    6687             : //}
+    6688             : 
+    6689             : /* //{ getMinZ() */
+    6690             : 
+    6691       25631 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6692             : 
+    6693       25631 :   if (!use_safety_area_) {
+    6694        1997 :     return std::numeric_limits<double>::lowest();
+    6695             :   }
+    6696             : 
+    6697       47268 :   geometry_msgs::PointStamped point;
+    6698             : 
+    6699       23634 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6700       23634 :   point.point.x         = 0;
+    6701       23634 :   point.point.y         = 0;
+    6702       23634 :   point.point.z         = _safety_area_min_z_;
+    6703             : 
+    6704       47268 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6705             : 
+    6706       23634 :   if (!ret) {
+    6707           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6708           0 :     return std::numeric_limits<double>::lowest();
+    6709             :   }
+    6710             : 
+    6711       23634 :   return ret->point.z;
+    6712             : }
+    6713             : 
+    6714             : //}
+    6715             : 
+    6716             : // | --------------------- obstacle bumper -------------------- |
+    6717             : 
+    6718             : /* bumperPushFromObstacle() //{ */
+    6719             : 
+    6720           0 : bool ControlManager::bumperPushFromObstacle(void) {
+    6721           0 :   if (!bumper_enabled_) {
+    6722           0 :     return true;
+    6723             :   }
+    6724             : 
+    6725           0 :   if (!sh_bumper_.hasMsg()) {
+    6726           0 :     return true;
+    6727             :   }
+    6728             : 
+    6729             :   // copy member variables
+    6730           0 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6731           0 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6732             : 
+    6733           0 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6734             : 
+    6735           0 :   double direction                     = 0;
+    6736           0 :   double repulsion_distance            = std::numeric_limits<double>::max();
+    6737           0 :   bool   horizontal_collision_detected = false;
+    6738             : 
+    6739           0 :   bool vertical_collision_detected = false;
+    6740             : 
+    6741           0 :   for (int i = 0; i < int(bumper_data->n_horizontal_sectors); i++) {
+    6742             : 
+    6743           0 :     if (bumper_data->sectors[i] < 0) {
+    6744           0 :       continue;
+    6745             :     }
+    6746             : 
+    6747           0 :     bool wall_locked_horizontal = false;
+    6748             : 
+    6749             :     // if the sector is under critical distance
+    6750           0 :     if (bumper_data->sectors[i] <= _bumper_horizontal_distance_ && bumper_data->sectors[i] < repulsion_distance) {
+    6751             : 
+    6752             :       // check for locking between the oposite walls
+    6753             :       // get the desired direction of motion
+    6754           0 :       double oposite_direction  = double(i) * sector_size + M_PI;
+    6755           0 :       int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6756             : 
+    6757           0 :       if (bumper_data->sectors[oposite_sector_idx] > 0 &&
+    6758           0 :           ((bumper_data->sectors[i] + bumper_data->sectors[oposite_sector_idx]) <= (2 * _bumper_horizontal_distance_ + 2 * _bumper_horizontal_overshoot_))) {
+    6759             : 
+    6760           0 :         wall_locked_horizontal = true;
+    6761             : 
+    6762           0 :         if (fabs(bumper_data->sectors[i] - bumper_data->sectors[oposite_sector_idx]) <= 2 * _bumper_horizontal_overshoot_) {
+    6763             : 
+    6764           0 :           ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: locked between two walls");
+    6765           0 :           continue;
+    6766             :         }
+    6767             :       }
+    6768             : 
+    6769             :       // get the id of the oposite sector
+    6770           0 :       direction = oposite_direction;
+    6771             : 
+    6772             :       /* int oposite_sector_idx = (i + bumper_data->n_horizontal_sectors / 2) % bumper_data->n_horizontal_sectors; */
+    6773             : 
+    6774           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %d vs. %d), obstacle distance: %.2f, repulsing", i,
+    6775             :                         oposite_sector_idx, bumper_data->sectors[i]);
+    6776             : 
+    6777           0 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: oposite direction: %.2f", oposite_direction);
+    6778             : 
+    6779           0 :       if (wall_locked_horizontal) {
+    6780           0 :         if (bumper_data->sectors[i] < bumper_data->sectors[oposite_sector_idx]) {
+    6781           0 :           repulsion_distance = _bumper_horizontal_overshoot_;
+    6782             :         } else {
+    6783           0 :           repulsion_distance = -_bumper_horizontal_overshoot_;
+    6784             :         }
+    6785             :       } else {
+    6786           0 :         repulsion_distance = _bumper_horizontal_distance_ + _bumper_horizontal_overshoot_ - bumper_data->sectors[i];
+    6787             :       }
+    6788             : 
+    6789           0 :       horizontal_collision_detected = true;
+    6790             :     }
+    6791             :   }
+    6792             : 
+    6793           0 :   bool   collision_above             = false;
+    6794           0 :   bool   collision_below             = false;
+    6795           0 :   double vertical_repulsion_distance = 0;
+    6796             : 
+    6797             :   // check for vertical collision down
+    6798           0 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors] > 0 && bumper_data->sectors[bumper_data->n_horizontal_sectors] <= _bumper_vertical_distance_) {
+    6799             : 
+    6800           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    6801           0 :     collision_above             = true;
+    6802           0 :     vertical_collision_detected = true;
+    6803           0 :     vertical_repulsion_distance = _bumper_vertical_distance_ - bumper_data->sectors[bumper_data->n_horizontal_sectors];
+    6804             :   }
+    6805             : 
+    6806             :   // check for vertical collision up
+    6807           0 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] > 0 &&
+    6808           0 :       bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] <= _bumper_vertical_distance_) {
+    6809             : 
+    6810           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    6811           0 :     collision_below             = true;
+    6812           0 :     vertical_collision_detected = true;
+    6813           0 :     vertical_repulsion_distance = -(_bumper_vertical_distance_ - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]);
+    6814             :   }
+    6815             : 
+    6816             :   // check the up/down wall locking
+    6817           0 :   if (collision_above && collision_below) {
+    6818             : 
+    6819           0 :     if (((bumper_data->sectors[bumper_data->n_horizontal_sectors] + bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) <=
+    6820           0 :          (2 * _bumper_vertical_distance_ + 2 * _bumper_vertical_overshoot_))) {
+    6821             : 
+    6822           0 :       vertical_repulsion_distance =
+    6823           0 :           (-bumper_data->sectors[bumper_data->n_horizontal_sectors] + bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) / 2.0;
+    6824             : 
+    6825           0 :       if (fabs(bumper_data->sectors[bumper_data->n_horizontal_sectors] - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) <=
+    6826           0 :           2 * _bumper_vertical_overshoot_) {
+    6827             : 
+    6828           0 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: locked between the floor and ceiling");
+    6829           0 :         vertical_collision_detected = false;
+    6830             :       }
+    6831             :     }
+    6832             :   }
+    6833             : 
+    6834             :   // if potential collision was detected and we should start the repulsing_
+    6835           0 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    6836             : 
+    6837           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: repulsion was initiated");
+    6838             : 
+    6839           0 :     if (!repulsing_) {
+    6840             : 
+    6841           0 :       if (_bumper_switch_tracker_) {
+    6842             : 
+    6843           0 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6844           0 :         std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6845             : 
+    6846             :         // remember the previously active tracker
+    6847           0 :         bumper_previous_tracker_ = active_tracker_name;
+    6848             : 
+    6849           0 :         if (active_tracker_name != _bumper_tracker_name_) {
+    6850             : 
+    6851           0 :           switchTracker(_bumper_tracker_name_);
+    6852             :         }
+    6853             :       }
+    6854             : 
+    6855           0 :       if (_bumper_switch_controller_) {
+    6856             : 
+    6857           0 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6858           0 :         std::string active_controller_name = _controller_names_[active_controller_idx];
+    6859             : 
+    6860             :         // remember the previously active controller
+    6861           0 :         bumper_previous_controller_ = active_controller_name;
+    6862             : 
+    6863           0 :         if (active_controller_name != _bumper_controller_name_) {
+    6864             : 
+    6865           0 :           switchController(_bumper_controller_name_);
+    6866             :         }
+    6867             :       }
+    6868             :     }
+    6869             : 
+    6870           0 :     repulsing_ = true;
+    6871             : 
+    6872           0 :     mrs_msgs::BumperStatus bumper_status;
+    6873           0 :     bumper_status.repulsing = repulsing_;
+    6874             : 
+    6875           0 :     ph_bumper_status_.publish(bumper_status);
+    6876             : 
+    6877           0 :     callbacks_enabled_ = false;
+    6878             : 
+    6879           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    6880             : 
+    6881           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6882             : 
+    6883             :     // create the reference in the fcu_untilted frame
+    6884           0 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    6885             : 
+    6886           0 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    6887             : 
+    6888           0 :     if (horizontal_collision_detected) {
+    6889           0 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    6890           0 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    6891             :     } else {
+    6892           0 :       reference_fcu_untilted.reference.position.x = 0;
+    6893           0 :       reference_fcu_untilted.reference.position.y = 0;
+    6894             :     }
+    6895             : 
+    6896           0 :     reference_fcu_untilted.reference.heading = 0;
+    6897             : 
+    6898           0 :     if (vertical_collision_detected) {
+    6899           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    6900             :     } else {
+    6901           0 :       reference_fcu_untilted.reference.position.z = 0;
+    6902             :     }
+    6903             : 
+    6904             :     {
+    6905           0 :       std::scoped_lock lock(mutex_tracker_list_);
+    6906             : 
+    6907             :       // transform the reference into the currently used frame
+    6908             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    6909             :       // to the tracker before we actually call the goto service
+    6910             : 
+    6911           0 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    6912             : 
+    6913           0 :       if (!ret) {
+    6914             : 
+    6915           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    6916           0 :         return false;
+    6917             :       }
+    6918             : 
+    6919           0 :       reference_fcu_untilted = ret.value();
+    6920             : 
+    6921             :       // copy the reference into the service type message
+    6922           0 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    6923           0 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    6924             : 
+    6925             :       // disable callbacks of all trackers
+    6926           0 :       req_enable_callbacks.data = false;
+    6927           0 :       for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6928           0 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6929             :       }
+    6930             : 
+    6931             :       // enable the callbacks for the active tracker
+    6932           0 :       req_enable_callbacks.data = true;
+    6933           0 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6934             : 
+    6935             :       // call the goto
+    6936           0 :       tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    6937           0 :           mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    6938             : 
+    6939             :       // disable the callbacks back again
+    6940           0 :       req_enable_callbacks.data = false;
+    6941           0 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6942             :     }
+    6943             :   }
+    6944             : 
+    6945             :   // if repulsing_ and the distance is safe once again
+    6946           0 :   if ((repulsing_ && !horizontal_collision_detected && !vertical_collision_detected)) {
+    6947             : 
+    6948           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: repulsion was stopped");
+    6949             : 
+    6950           0 :     if (_bumper_switch_tracker_) {
+    6951             : 
+    6952           0 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6953           0 :       std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6954             : 
+    6955           0 :       if (active_tracker_name != bumper_previous_tracker_) {
+    6956             : 
+    6957           0 :         switchTracker(bumper_previous_tracker_);
+    6958             :       }
+    6959             :     }
+    6960             : 
+    6961           0 :     if (_bumper_switch_controller_) {
+    6962             : 
+    6963           0 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6964           0 :       std::string active_controller_name = _controller_names_[active_controller_idx];
+    6965             : 
+    6966           0 :       if (active_controller_name != bumper_previous_controller_) {
+    6967             : 
+    6968           0 :         switchController(bumper_previous_controller_);
+    6969             :       }
+    6970             :     }
+    6971             : 
+    6972           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6973             : 
+    6974             :     {
+    6975           0 :       std::scoped_lock lock(mutex_tracker_list_);
+    6976             : 
+    6977             :       // enable callbacks of all trackers
+    6978           0 :       req_enable_callbacks.data = true;
+    6979           0 :       for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6980           0 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6981             :       }
+    6982             :     }
+    6983             : 
+    6984           0 :     callbacks_enabled_ = true;
+    6985             : 
+    6986           0 :     repulsing_ = false;
+    6987             :   }
+    6988             : 
+    6989           0 :   return false;
+    6990             : }
+    6991             : 
+    6992             : //}
+    6993             : 
+    6994             : /* bumperGetSectorId() //{ */
+    6995             : 
+    6996           0 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    6997             :   // copy member variables
+    6998           0 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6999             : 
+    7000             :   // heading of the point in drone frame
+    7001           0 :   double point_heading_horizontal = atan2(y, x);
+    7002             : 
+    7003           0 :   point_heading_horizontal += TAU;
+    7004             : 
+    7005             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7006           0 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7007           0 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7008             :   }
+    7009             : 
+    7010             :   // heading of the right edge of the first sector
+    7011           0 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7012             : 
+    7013             :   // calculate the idx
+    7014           0 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7015             : 
+    7016           0 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7017           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7018             :   }
+    7019             : 
+    7020           0 :   return idx;
+    7021             : }
+    7022             : 
+    7023             : //}
+    7024             : 
+    7025             : // | ------------------------- safety ------------------------- |
+    7026             : 
+    7027             : /* //{ changeLandingState() */
+    7028             : 
+    7029           6 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7030             :   // copy member variables
+    7031          12 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7032             : 
+    7033             :   {
+    7034           6 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7035             : 
+    7036           6 :     previous_state_landing_ = current_state_landing_;
+    7037           6 :     current_state_landing_  = new_state;
+    7038             :   }
+    7039             : 
+    7040           6 :   switch (current_state_landing_) {
+    7041             : 
+    7042           3 :     case IDLE_STATE:
+    7043           3 :       break;
+    7044           3 :     case LANDING_STATE: {
+    7045             : 
+    7046           3 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7047           3 :       timer_eland_.start();
+    7048           3 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7049           3 :       eland_triggered_ = true;
+    7050           3 :       bumper_enabled_  = false;
+    7051             : 
+    7052           3 :       landing_uav_mass_ = getMass();
+    7053             :     }
+    7054             : 
+    7055           3 :     break;
+    7056             :   }
+    7057             : 
+    7058           6 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7059           6 : }
+    7060             : 
+    7061             : //}
+    7062             : 
+    7063             : /* hover() //{ */
+    7064             : 
+    7065           0 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7066           0 :   if (!is_initialized_)
+    7067           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7068             : 
+    7069           0 :   if (eland_triggered_)
+    7070           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7071             : 
+    7072           0 :   if (failsafe_triggered_)
+    7073           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7074             : 
+    7075             :   {
+    7076           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7077             : 
+    7078           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7079           0 :     std_srvs::TriggerRequest            request;
+    7080             : 
+    7081           0 :     response = tracker_list_[active_tracker_idx_]->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7082             : 
+    7083           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7084             : 
+    7085           0 :       return std::tuple(response->success, response->message);
+    7086             : 
+    7087             :     } else {
+    7088             : 
+    7089           0 :       std::stringstream ss;
+    7090           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'hover()' function!";
+    7091             : 
+    7092           0 :       return std::tuple(false, ss.str());
+    7093             :     }
+    7094             :   }
+    7095             : }
+    7096             : 
+    7097             : //}
+    7098             : 
+    7099             : /* //{ ehover() */
+    7100             : 
+    7101           0 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7102           0 :   if (!is_initialized_)
+    7103           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7104             : 
+    7105           0 :   if (eland_triggered_)
+    7106           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7107             : 
+    7108           0 :   if (failsafe_triggered_)
+    7109           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7110             : 
+    7111             :   // copy the member variables
+    7112           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7113           0 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7114           0 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7115             : 
+    7116           0 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7117             : 
+    7118           0 :     std::stringstream ss;
+    7119           0 :     ss << "can not trigger ehover while not flying";
+    7120           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7121             : 
+    7122           0 :     return std::tuple(false, ss.str());
+    7123             :   }
+    7124             : 
+    7125           0 :   ungripSrv();
+    7126             : 
+    7127             :   {
+    7128             : 
+    7129           0 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7130             : 
+    7131             :     // check if the tracker was successfully switched
+    7132             :     // this is vital, that is the core of the hover
+    7133           0 :     if (!success) {
+    7134             : 
+    7135           0 :       std::stringstream ss;
+    7136           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7137           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7138             : 
+    7139           0 :       return std::tuple(success, ss.str());
+    7140             :     }
+    7141             :   }
+    7142             : 
+    7143             :   {
+    7144           0 :     auto [success, message] = switchController(_eland_controller_name_);
+    7145             : 
+    7146             :     // check if the controller was successfully switched
+    7147             :     // this is not vital, we can continue without that
+    7148           0 :     if (!success) {
+    7149             : 
+    7150           0 :       std::stringstream ss;
+    7151           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7152           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7153             :     }
+    7154             :   }
+    7155             : 
+    7156           0 :   std::stringstream ss;
+    7157           0 :   ss << "ehover activated";
+    7158           0 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7159             : 
+    7160           0 :   callbacks_enabled_ = false;
+    7161             : 
+    7162           0 :   return std::tuple(true, ss.str());
+    7163             : }
+    7164             : 
+    7165             : //}
+    7166             : 
+    7167             : /* eland() //{ */
+    7168             : 
+    7169           3 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7170           3 :   if (!is_initialized_)
+    7171           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7172             : 
+    7173           3 :   if (eland_triggered_)
+    7174           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7175             : 
+    7176           3 :   if (failsafe_triggered_)
+    7177           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7178             : 
+    7179             :   // copy member variables
+    7180           6 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7181           6 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7182           3 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7183             : 
+    7184           3 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7185             : 
+    7186           0 :     std::stringstream ss;
+    7187           0 :     ss << "can not trigger eland while not flying";
+    7188           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7189             : 
+    7190           0 :     return std::tuple(false, ss.str());
+    7191             :   }
+    7192             : 
+    7193           3 :   if (_rc_emergency_handoff_) {
+    7194             : 
+    7195           0 :     toggleOutput(false);
+    7196             : 
+    7197           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7198             :   }
+    7199             : 
+    7200             :   {
+    7201           3 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7202             : 
+    7203             :     // check if the tracker was successfully switched
+    7204             :     // this is vital
+    7205           3 :     if (!success) {
+    7206             : 
+    7207           0 :       std::stringstream ss;
+    7208           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7209           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7210             : 
+    7211           0 :       return std::tuple(success, ss.str());
+    7212             :     }
+    7213             :   }
+    7214             : 
+    7215             :   {
+    7216           6 :     auto [success, message] = switchController(_eland_controller_name_);
+    7217             : 
+    7218             :     // check if the controller was successfully switched
+    7219             :     // this is not vital, we can continue without it
+    7220           3 :     if (!success) {
+    7221             : 
+    7222           0 :       std::stringstream ss;
+    7223           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7224           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7225             :     }
+    7226             :   }
+    7227             : 
+    7228             :   // | ----------------- call the eland service ----------------- |
+    7229             : 
+    7230           3 :   std::stringstream ss;
+    7231             :   bool              success;
+    7232             : 
+    7233           3 :   if (elandSrv()) {
+    7234             : 
+    7235           3 :     changeLandingState(LANDING_STATE);
+    7236             : 
+    7237           3 :     odometryCallbacksSrv(false);
+    7238             : 
+    7239           3 :     ss << "eland activated";
+    7240           3 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7241             : 
+    7242           3 :     success = true;
+    7243             : 
+    7244           3 :     callbacks_enabled_ = false;
+    7245             : 
+    7246             :   } else {
+    7247             : 
+    7248           0 :     ss << "error during activation of eland";
+    7249           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7250             : 
+    7251           0 :     success = false;
+    7252             :   }
+    7253             : 
+    7254           6 :   return std::tuple(success, ss.str());
+    7255             : }
+    7256             : 
+    7257             : //}
+    7258             : 
+    7259             : /* failsafe() //{ */
+    7260             : 
+    7261           1 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7262             :   // copy member variables
+    7263           2 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7264           2 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7265           1 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7266           1 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7267             : 
+    7268           1 :   if (!is_initialized_) {
+    7269           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7270             :   }
+    7271             : 
+    7272           1 :   if (failsafe_triggered_) {
+    7273           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7274             :   }
+    7275             : 
+    7276           1 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7277             : 
+    7278           0 :     std::stringstream ss;
+    7279           0 :     ss << "can not trigger failsafe while not flying";
+    7280           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7281           0 :     return std::tuple(false, ss.str());
+    7282             :   }
+    7283             : 
+    7284           1 :   if (_rc_emergency_handoff_) {
+    7285             : 
+    7286           0 :     toggleOutput(false);
+    7287             : 
+    7288           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7289             :   }
+    7290             : 
+    7291           1 :   if (_parachute_enabled_) {
+    7292             : 
+    7293           0 :     auto [success, message] = deployParachute();
+    7294             : 
+    7295           0 :     if (success) {
+    7296             : 
+    7297           0 :       std::stringstream ss;
+    7298           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7299           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7300             : 
+    7301           0 :       return std::tuple(true, ss.str());
+    7302             : 
+    7303             :     } else {
+    7304             : 
+    7305           0 :       std::stringstream ss;
+    7306           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7307           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7308             :     }
+    7309             :   }
+    7310             : 
+    7311           1 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7312             : 
+    7313             :     try {
+    7314             : 
+    7315           2 :       std::scoped_lock lock(mutex_controller_list_);
+    7316             : 
+    7317           1 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7318           1 :       controller_list_[_failsafe_controller_idx_]->activate(last_control_output);
+    7319             : 
+    7320             :       {
+    7321           2 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7322             : 
+    7323             :         // update the time (used in failsafe)
+    7324           1 :         controller_tracker_switch_time_ = ros::Time::now();
+    7325             :       }
+    7326             : 
+    7327           1 :       failsafe_triggered_ = true;
+    7328           1 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7329           1 :       timer_eland_.stop();
+    7330           1 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7331             : 
+    7332           1 :       landing_uav_mass_ = getMass();
+    7333             : 
+    7334           1 :       eland_triggered_ = false;
+    7335           1 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7336           1 :       timer_failsafe_.start();
+    7337           1 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7338             : 
+    7339           1 :       bumper_enabled_ = false;
+    7340             : 
+    7341           1 :       odometryCallbacksSrv(false);
+    7342             : 
+    7343           1 :       callbacks_enabled_ = false;
+    7344             : 
+    7345           1 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7346             : 
+    7347             :       // super important, switch the active controller idx
+    7348             :       try {
+    7349           1 :         controller_list_[active_controller_idx_]->deactivate();
+    7350           1 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7351             :       }
+    7352           0 :       catch (std::runtime_error& exrun) {
+    7353           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    7354             :       }
+    7355             :     }
+    7356           0 :     catch (std::runtime_error& exrun) {
+    7357           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7358           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7359             :     }
+    7360             :   }
+    7361             : 
+    7362           2 :   return std::tuple(true, "failsafe activated");
+    7363             : }
+    7364             : 
+    7365             : //}
+    7366             : 
+    7367             : /* escalatingFailsafe() //{ */
+    7368             : 
+    7369           0 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7370           0 :   std::stringstream ss;
+    7371             : 
+    7372           0 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7373             : 
+    7374           0 :     ss << "too soon for escalating failsafe";
+    7375           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7376             : 
+    7377           0 :     return std::tuple(false, ss.str());
+    7378             :   }
+    7379             : 
+    7380           0 :   if (!output_enabled_) {
+    7381             : 
+    7382           0 :     ss << "not escalating failsafe, output is disabled";
+    7383           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7384             : 
+    7385           0 :     return std::tuple(false, ss.str());
+    7386             :   }
+    7387             : 
+    7388           0 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7389             : 
+    7390           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7391           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7392             : 
+    7393           0 :   std::string active_tracker_name    = _tracker_names_[active_tracker_idx];
+    7394           0 :   std::string active_controller_name = _controller_names_[active_controller_idx];
+    7395             : 
+    7396           0 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7397             : 
+    7398           0 :   escalating_failsafe_time_ = ros::Time::now();
+    7399             : 
+    7400           0 :   switch (next_state) {
+    7401             : 
+    7402           0 :     case ESC_NONE_STATE: {
+    7403             : 
+    7404           0 :       ss << "escalating failsafe has run to impossible situation";
+    7405           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7406             : 
+    7407           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7408             : 
+    7409             :       break;
+    7410             :     }
+    7411             : 
+    7412           0 :     case ESC_EHOVER_STATE: {
+    7413             : 
+    7414           0 :       ss << "escalating failsafe escalates to ehover";
+    7415           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7416             : 
+    7417           0 :       auto [success, message] = ehover();
+    7418             : 
+    7419           0 :       if (success) {
+    7420           0 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7421             :       }
+    7422             : 
+    7423           0 :       return {success, message};
+    7424             : 
+    7425             :       break;
+    7426             :     }
+    7427             : 
+    7428           0 :     case ESC_ELAND_STATE: {
+    7429             : 
+    7430           0 :       ss << "escalating failsafe escalates to eland";
+    7431           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7432             : 
+    7433           0 :       auto [success, message] = eland();
+    7434             : 
+    7435           0 :       if (success) {
+    7436           0 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7437             :       }
+    7438             : 
+    7439           0 :       return {success, message};
+    7440             : 
+    7441             :       break;
+    7442             :     }
+    7443             : 
+    7444           0 :     case ESC_FAILSAFE_STATE: {
+    7445             : 
+    7446           0 :       escalating_failsafe_time_ = ros::Time::now();
+    7447             : 
+    7448           0 :       ss << "escalating failsafe escalates to failsafe";
+    7449           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7450             : 
+    7451           0 :       auto [success, message] = failsafe();
+    7452             : 
+    7453           0 :       if (success) {
+    7454           0 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7455             :       }
+    7456             : 
+    7457           0 :       return {success, message};
+    7458             : 
+    7459             :       break;
+    7460             :     }
+    7461             : 
+    7462           0 :     case ESC_FINISHED_STATE: {
+    7463             : 
+    7464           0 :       escalating_failsafe_time_ = ros::Time::now();
+    7465             : 
+    7466           0 :       ss << "escalating failsafe has nothing more to do";
+    7467           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7468             : 
+    7469           0 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7470             : 
+    7471             :       break;
+    7472             :     }
+    7473             : 
+    7474           0 :     default: {
+    7475             : 
+    7476           0 :       break;
+    7477             :     }
+    7478             :   }
+    7479             : 
+    7480           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7481             : 
+    7482           0 :   return std::tuple(false, "escalating failsafe exception");
+    7483             : }
+    7484             : 
+    7485             : //}
+    7486             : 
+    7487             : /* getNextEscFailsafeState() //{ */
+    7488             : 
+    7489           0 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7490           0 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7491             : 
+    7492           0 :   switch (current_state) {
+    7493             : 
+    7494           0 :     case ESC_FINISHED_STATE: {
+    7495             : 
+    7496           0 :       return ESC_FINISHED_STATE;
+    7497             : 
+    7498             :       break;
+    7499             :     }
+    7500             : 
+    7501           0 :     case ESC_NONE_STATE: {
+    7502             : 
+    7503           0 :       if (_escalating_failsafe_ehover_) {
+    7504           0 :         return ESC_EHOVER_STATE;
+    7505           0 :       } else if (_escalating_failsafe_eland_) {
+    7506           0 :         return ESC_ELAND_STATE;
+    7507           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7508           0 :         return ESC_FAILSAFE_STATE;
+    7509             :       } else {
+    7510           0 :         return ESC_FINISHED_STATE;
+    7511             :       }
+    7512             : 
+    7513             :       break;
+    7514             :     }
+    7515             : 
+    7516           0 :     case ESC_EHOVER_STATE: {
+    7517             : 
+    7518           0 :       if (_escalating_failsafe_eland_) {
+    7519           0 :         return ESC_ELAND_STATE;
+    7520           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7521           0 :         return ESC_FAILSAFE_STATE;
+    7522             :       } else {
+    7523           0 :         return ESC_FINISHED_STATE;
+    7524             :       }
+    7525             : 
+    7526             :       break;
+    7527             :     }
+    7528             : 
+    7529           0 :     case ESC_ELAND_STATE: {
+    7530             : 
+    7531           0 :       if (_escalating_failsafe_failsafe_) {
+    7532           0 :         return ESC_FAILSAFE_STATE;
+    7533             :       } else {
+    7534           0 :         return ESC_FINISHED_STATE;
+    7535             :       }
+    7536             : 
+    7537             :       break;
+    7538             :     }
+    7539             : 
+    7540           0 :     case ESC_FAILSAFE_STATE: {
+    7541             : 
+    7542           0 :       return ESC_FINISHED_STATE;
+    7543             : 
+    7544             :       break;
+    7545             :     }
+    7546             :   }
+    7547             : 
+    7548           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7549             : 
+    7550           0 :   return ESC_NONE_STATE;
+    7551             : }
+    7552             : 
+    7553             : //}
+    7554             : 
+    7555             : // | ------------------- trajectory tracking ------------------ |
+    7556             : 
+    7557             : /* startTrajectoryTracking() //{ */
+    7558             : 
+    7559           0 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7560           0 :   if (!is_initialized_)
+    7561           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7562             : 
+    7563             :   {
+    7564           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7565             : 
+    7566           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7567           0 :     std_srvs::TriggerRequest            request;
+    7568             : 
+    7569             :     response =
+    7570           0 :         tracker_list_[active_tracker_idx_]->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7571             : 
+    7572           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7573             : 
+    7574           0 :       return std::tuple(response->success, response->message);
+    7575             : 
+    7576             :     } else {
+    7577             : 
+    7578           0 :       std::stringstream ss;
+    7579           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'startTrajectoryTracking()' function!";
+    7580             : 
+    7581           0 :       return std::tuple(false, ss.str());
+    7582             :     }
+    7583             :   }
+    7584             : }
+    7585             : 
+    7586             : //}
+    7587             : 
+    7588             : /* stopTrajectoryTracking() //{ */
+    7589             : 
+    7590           0 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7591           0 :   if (!is_initialized_)
+    7592           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7593             : 
+    7594             :   {
+    7595           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7596             : 
+    7597           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7598           0 :     std_srvs::TriggerRequest            request;
+    7599             : 
+    7600             :     response =
+    7601           0 :         tracker_list_[active_tracker_idx_]->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7602             : 
+    7603           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7604             : 
+    7605           0 :       return std::tuple(response->success, response->message);
+    7606             : 
+    7607             :     } else {
+    7608             : 
+    7609           0 :       std::stringstream ss;
+    7610           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7611             : 
+    7612           0 :       return std::tuple(false, ss.str());
+    7613             :     }
+    7614             :   }
+    7615             : }
+    7616             : 
+    7617             : //}
+    7618             : 
+    7619             : /* resumeTrajectoryTracking() //{ */
+    7620             : 
+    7621           0 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7622           0 :   if (!is_initialized_)
+    7623           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7624             : 
+    7625             :   {
+    7626           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7627             : 
+    7628           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7629           0 :     std_srvs::TriggerRequest            request;
+    7630             : 
+    7631             :     response =
+    7632           0 :         tracker_list_[active_tracker_idx_]->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7633             : 
+    7634           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7635             : 
+    7636           0 :       return std::tuple(response->success, response->message);
+    7637             : 
+    7638             :     } else {
+    7639             : 
+    7640           0 :       std::stringstream ss;
+    7641           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7642             : 
+    7643           0 :       return std::tuple(false, ss.str());
+    7644             :     }
+    7645             :   }
+    7646             : }
+    7647             : 
+    7648             : //}
+    7649             : 
+    7650             : /* gotoTrajectoryStart() //{ */
+    7651             : 
+    7652           0 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7653           0 :   if (!is_initialized_)
+    7654           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7655             : 
+    7656             :   {
+    7657           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7658             : 
+    7659           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7660           0 :     std_srvs::TriggerRequest            request;
+    7661             : 
+    7662           0 :     response = tracker_list_[active_tracker_idx_]->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7663             : 
+    7664           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7665             : 
+    7666           0 :       return std::tuple(response->success, response->message);
+    7667             : 
+    7668             :     } else {
+    7669             : 
+    7670           0 :       std::stringstream ss;
+    7671           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7672             : 
+    7673           0 :       return std::tuple(false, ss.str());
+    7674             :     }
+    7675             :   }
+    7676             : }
+    7677             : 
+    7678             : //}
+    7679             : 
+    7680             : // | ----------------- service client wrappers ---------------- |
+    7681             : 
+    7682             : /* arming() //{ */
+    7683             : 
+    7684           6 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7685          12 :   std::stringstream ss;
+    7686             : 
+    7687           6 :   if (input) {
+    7688             : 
+    7689           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7690           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7691           0 :     return std::tuple(false, ss.str());
+    7692             :   }
+    7693             : 
+    7694           6 :   if (!input && !isOffboard()) {
+    7695             : 
+    7696           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7697           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7698           0 :     return std::tuple(false, ss.str());
+    7699             :   }
+    7700             : 
+    7701           6 :   if (!input && _rc_emergency_handoff_) {
+    7702             : 
+    7703           0 :     toggleOutput(false);
+    7704             : 
+    7705           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7706             :   }
+    7707             : 
+    7708           6 :   std_srvs::SetBool srv_out;
+    7709             : 
+    7710           6 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7711             : 
+    7712           6 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7713             : 
+    7714           6 :   if (sch_arming_.call(srv_out)) {
+    7715             : 
+    7716           6 :     if (srv_out.response.success) {
+    7717             : 
+    7718           6 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7719           6 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7720             : 
+    7721           6 :       if (!input) {
+    7722             : 
+    7723           6 :         toggleOutput(false);
+    7724             : 
+    7725           6 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7726           6 :         timer_failsafe_.stop();
+    7727           6 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7728             : 
+    7729           6 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7730           6 :         timer_eland_.stop();
+    7731           6 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7732             : 
+    7733           6 :         shutdown();
+    7734             :       }
+    7735             : 
+    7736             :     } else {
+    7737           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7738           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7739             :     }
+    7740             : 
+    7741             :   } else {
+    7742           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7743           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7744             :   }
+    7745             : 
+    7746          12 :   return std::tuple(srv_out.response.success, ss.str());
+    7747             : }
+    7748             : 
+    7749             : //}
+    7750             : 
+    7751             : /* odometryCallbacksSrv() //{ */
+    7752             : 
+    7753           4 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7754           4 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7755             : 
+    7756           8 :   std_srvs::SetBool srv;
+    7757             : 
+    7758           4 :   srv.request.data = input;
+    7759             : 
+    7760           4 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7761             : 
+    7762           4 :   if (res) {
+    7763             : 
+    7764           4 :     if (!srv.response.success) {
+    7765           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7766             :     }
+    7767             : 
+    7768             :   } else {
+    7769           0 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7770             :   }
+    7771           4 : }
+    7772             : 
+    7773             : //}
+    7774             : 
+    7775             : /* elandSrv() //{ */
+    7776             : 
+    7777           3 : bool ControlManager::elandSrv(void) {
+    7778           3 :   ROS_INFO("[ControlManager]: calling for eland");
+    7779             : 
+    7780           6 :   std_srvs::Trigger srv;
+    7781             : 
+    7782           3 :   bool res = sch_eland_.call(srv);
+    7783             : 
+    7784           3 :   if (res) {
+    7785             : 
+    7786           3 :     if (!srv.response.success) {
+    7787           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    7788             :     }
+    7789             : 
+    7790           3 :     return srv.response.success;
+    7791             : 
+    7792             :   } else {
+    7793             : 
+    7794           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    7795             : 
+    7796           0 :     return false;
+    7797             :   }
+    7798             : }
+    7799             : 
+    7800             : //}
+    7801             : 
+    7802             : /* shutdown() //{ */
+    7803             : 
+    7804           9 : void ControlManager::shutdown() {
+    7805             :   // copy member variables
+    7806          18 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    7807             : 
+    7808           9 :   if (_automatic_pc_shutdown_enabled_) {
+    7809             : 
+    7810           0 :     ROS_INFO("[ControlManager]: calling service for PC shutdown");
+    7811             : 
+    7812           0 :     std_srvs::Trigger shutdown_out;
+    7813           0 :     sch_shutdown_.call(shutdown_out);
+    7814             :   }
+    7815           9 : }
+    7816             : 
+    7817             : //}
+    7818             : 
+    7819             : /* parachuteSrv() //{ */
+    7820             : 
+    7821           0 : bool ControlManager::parachuteSrv(void) {
+    7822           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    7823             : 
+    7824           0 :   std_srvs::Trigger srv;
+    7825             : 
+    7826           0 :   bool res = sch_parachute_.call(srv);
+    7827             : 
+    7828           0 :   if (res) {
+    7829             : 
+    7830           0 :     if (!srv.response.success) {
+    7831           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    7832             :     }
+    7833             : 
+    7834           0 :     return srv.response.success;
+    7835             : 
+    7836             :   } else {
+    7837             : 
+    7838           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    7839             : 
+    7840           0 :     return false;
+    7841             :   }
+    7842             : }
+    7843             : 
+    7844             : //}
+    7845             : 
+    7846             : /* ungripSrv() //{ */
+    7847             : 
+    7848         283 : void ControlManager::ungripSrv(void) {
+    7849         566 :   std_srvs::Trigger srv;
+    7850             : 
+    7851         283 :   bool res = sch_ungrip_.call(srv);
+    7852             : 
+    7853         283 :   if (res) {
+    7854             : 
+    7855           0 :     if (!srv.response.success) {
+    7856           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    7857             :     }
+    7858             : 
+    7859             :   } else {
+    7860         283 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    7861             :   }
+    7862         283 : }
+    7863             : 
+    7864             : //}
+    7865             : 
+    7866             : // | ------------------------ routines ------------------------ |
+    7867             : 
+    7868             : /* toggleOutput() //{ */
+    7869             : 
+    7870          48 : void ControlManager::toggleOutput(const bool& input) {
+    7871             : 
+    7872          48 :   if (input == output_enabled_) {
+    7873           3 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    7874           3 :     return;
+    7875             :   }
+    7876             : 
+    7877          45 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    7878             : 
+    7879          45 :   output_enabled_ = input;
+    7880             : 
+    7881             :   // if switching output off, switch to NullTracker
+    7882          45 :   if (!output_enabled_) {
+    7883             : 
+    7884           6 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    7885             : 
+    7886           6 :     switchTracker(_null_tracker_name_);
+    7887             : 
+    7888           6 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    7889             : 
+    7890           6 :     switchController(_eland_controller_name_);
+    7891             : 
+    7892             :     // | --------- deactivate all trackers and controllers -------- |
+    7893             : 
+    7894          42 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    7895             : 
+    7896          36 :       std::map<std::string, TrackerParams>::iterator it;
+    7897          36 :       it = trackers_.find(_tracker_names_[i]);
+    7898             : 
+    7899             :       try {
+    7900          36 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    7901          36 :         tracker_list_[i]->deactivate();
+    7902             :       }
+    7903           0 :       catch (std::runtime_error& ex) {
+    7904           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    7905             :       }
+    7906             :     }
+    7907             : 
+    7908          36 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    7909             : 
+    7910          30 :       std::map<std::string, ControllerParams>::iterator it;
+    7911          30 :       it = controllers_.find(_controller_names_[i]);
+    7912             : 
+    7913             :       try {
+    7914          30 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    7915          30 :         controller_list_[i]->deactivate();
+    7916             :       }
+    7917           0 :       catch (std::runtime_error& ex) {
+    7918           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    7919             :       }
+    7920             :     }
+    7921             : 
+    7922           6 :     timer_failsafe_.stop();
+    7923           6 :     timer_eland_.stop();
+    7924           6 :     timer_pirouette_.stop();
+    7925             : 
+    7926           6 :     offboard_mode_was_true_ = false;
+    7927             :   }
+    7928             : }
+    7929             : 
+    7930             : //}
+    7931             : 
+    7932             : /* switchTracker() //{ */
+    7933             : 
+    7934          91 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    7935         273 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    7936         273 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    7937             : 
+    7938             :   // copy member variables
+    7939         182 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7940          91 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7941             : 
+    7942         182 :   std::stringstream ss;
+    7943             : 
+    7944          91 :   if (!got_uav_state_) {
+    7945             : 
+    7946           0 :     ss << "can not switch tracker, missing odometry!";
+    7947           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7948           0 :     return std::tuple(false, ss.str());
+    7949             :   }
+    7950             : 
+    7951          91 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    7952             : 
+    7953           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    7954           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7955           0 :     return std::tuple(false, ss.str());
+    7956             :   }
+    7957             : 
+    7958          91 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    7959             : 
+    7960             :   // check if the tracker exists
+    7961          91 :   if (!new_tracker_idx) {
+    7962           0 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    7963           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7964           0 :     return std::tuple(false, ss.str());
+    7965             :   }
+    7966             : 
+    7967             :   // check if the tracker is already active
+    7968          91 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    7969           2 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    7970           2 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7971           2 :     return std::tuple(true, ss.str());
+    7972             :   }
+    7973             : 
+    7974             :   {
+    7975          89 :     std::scoped_lock lock(mutex_tracker_list_);
+    7976             : 
+    7977             :     try {
+    7978             : 
+    7979          89 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_[new_tracker_idx.value()].c_str());
+    7980             : 
+    7981          89 :       auto [success, message] = tracker_list_[new_tracker_idx.value()]->activate(last_tracker_cmd);
+    7982             : 
+    7983          89 :       if (!success) {
+    7984             : 
+    7985           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    7986           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7987           0 :         return std::tuple(false, ss.str());
+    7988             : 
+    7989             :       } else {
+    7990             : 
+    7991          89 :         ss << "the tracker '" << tracker_name << "' was activated";
+    7992          89 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7993             : 
+    7994             :         {
+    7995         178 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7996             : 
+    7997             :           // update the time (used in failsafe)
+    7998          89 :           controller_tracker_switch_time_ = ros::Time::now();
+    7999             :         }
+    8000             : 
+    8001             :         // super important, switch the active tracker idx
+    8002             :         try {
+    8003             : 
+    8004          89 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8005          89 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8006             : 
+    8007             :           // if switching from null tracker, re-activate the already active the controller
+    8008          89 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    8009             : 
+    8010          39 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8011             :             {
+    8012          78 :               std::scoped_lock lock(mutex_controller_list_);
+    8013             : 
+    8014          39 :               initializeControlOutput();
+    8015             : 
+    8016          78 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8017             : 
+    8018          39 :               controller_list_[active_controller_idx_]->activate(last_control_output);
+    8019             : 
+    8020             :               {
+    8021          78 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8022             : 
+    8023             :                 // update the time (used in failsafe)
+    8024          39 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8025             :               }
+    8026             :             }
+    8027             : 
+    8028             :             // if switching to null tracker, deactivate the active controller
+    8029          50 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8030             : 
+    8031           6 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8032             :             {
+    8033          12 :               std::scoped_lock lock(mutex_controller_list_);
+    8034             : 
+    8035           6 :               controller_list_[active_controller_idx_]->deactivate();
+    8036             :             }
+    8037             : 
+    8038             :             {
+    8039           6 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8040             : 
+    8041           6 :               last_tracker_cmd_ = {};
+    8042             :             }
+    8043             : 
+    8044           6 :             initializeControlOutput();
+    8045             :           }
+    8046             : 
+    8047          89 :           active_tracker_idx_ = new_tracker_idx.value();
+    8048             :         }
+    8049           0 :         catch (std::runtime_error& exrun) {
+    8050           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8051             :         }
+    8052             :       }
+    8053             :     }
+    8054           0 :     catch (std::runtime_error& exrun) {
+    8055           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8056           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8057             :     }
+    8058             :   }
+    8059             : 
+    8060          89 :   return std::tuple(true, ss.str());
+    8061             : }
+    8062             : 
+    8063             : //}
+    8064             : 
+    8065             : /* switchController() //{ */
+    8066             : 
+    8067          89 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8068         267 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8069         267 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8070             : 
+    8071             :   // copy member variables
+    8072         178 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8073         178 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8074          89 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8075             : 
+    8076         178 :   std::stringstream ss;
+    8077             : 
+    8078          89 :   if (!got_uav_state_) {
+    8079             : 
+    8080           0 :     ss << "can not switch controller, missing odometry!";
+    8081           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8082           0 :     return std::tuple(false, ss.str());
+    8083             :   }
+    8084             : 
+    8085          89 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8086             : 
+    8087           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8088           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8089           0 :     return std::tuple(false, ss.str());
+    8090             :   }
+    8091             : 
+    8092          89 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8093             : 
+    8094             :   // check if the controller exists
+    8095          89 :   if (!new_controller_idx) {
+    8096           0 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8097           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8098           0 :     return std::tuple(false, ss.str());
+    8099             :   }
+    8100             : 
+    8101             :   // check if the controller is not active
+    8102          89 :   if (new_controller_idx.value() == active_controller_idx) {
+    8103             : 
+    8104          11 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8105          11 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8106          11 :     return std::tuple(true, ss.str());
+    8107             :   }
+    8108             : 
+    8109             :   {
+    8110          78 :     std::scoped_lock lock(mutex_controller_list_);
+    8111             : 
+    8112             :     try {
+    8113             : 
+    8114          78 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_[new_controller_idx.value()].c_str());
+    8115          78 :       if (!controller_list_[new_controller_idx.value()]->activate(last_control_output)) {
+    8116             : 
+    8117           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8118           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8119           0 :         return std::tuple(false, ss.str());
+    8120             : 
+    8121             :       } else {
+    8122             : 
+    8123          78 :         ss << "the controller '" << controller_name << "' was activated";
+    8124          78 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8125             : 
+    8126          78 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_[new_controller_idx.value()].c_str(),
+    8127             :                  _tracker_names_[active_tracker_idx_].c_str());
+    8128             : 
+    8129             :         // reactivate the current tracker
+    8130             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8131             :         // but it serves the purpose
+    8132             :         {
+    8133          78 :           std::scoped_lock lock(mutex_tracker_list_);
+    8134             : 
+    8135          78 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8136          78 :           tracker_list_[active_tracker_idx_]->activate({});
+    8137             :         }
+    8138             : 
+    8139             :         {
+    8140         156 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8141             : 
+    8142             :           // update the time (used in failsafe)
+    8143          78 :           controller_tracker_switch_time_ = ros::Time::now();
+    8144             :         }
+    8145             : 
+    8146             :         // super important, switch the active controller idx
+    8147             :         try {
+    8148             : 
+    8149          78 :           controller_list_[active_controller_idx_]->deactivate();
+    8150          78 :           active_controller_idx_ = new_controller_idx.value();
+    8151             :         }
+    8152           0 :         catch (std::runtime_error& exrun) {
+    8153           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    8154             :         }
+    8155             :       }
+    8156             :     }
+    8157           0 :     catch (std::runtime_error& exrun) {
+    8158           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8159           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8160             :     }
+    8161             :   }
+    8162             : 
+    8163          78 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8164             : 
+    8165             :   {
+    8166          78 :     std::scoped_lock lock(mutex_constraints_);
+    8167             : 
+    8168          78 :     sanitized_constraints_ = current_constraints_;
+    8169          78 :     sanitized_constraints  = sanitized_constraints_;
+    8170             :   }
+    8171             : 
+    8172          78 :   setConstraintsToControllers(sanitized_constraints);
+    8173             : 
+    8174          78 :   return std::tuple(true, ss.str());
+    8175             : }
+    8176             : 
+    8177             : //}
+    8178             : 
+    8179             : /* updateTrackers() //{ */
+    8180             : 
+    8181       39318 : void ControlManager::updateTrackers(void) {
+    8182       78636 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8183       78636 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8184             : 
+    8185             :   // copy member variables
+    8186       39318 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8187       39318 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8188       39318 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8189             : 
+    8190             :   // --------------------------------------------------------------
+    8191             :   // |                     Update the trackers                    |
+    8192             :   // --------------------------------------------------------------
+    8193             : 
+    8194           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8195             : 
+    8196             :   // for each tracker
+    8197      277262 :   for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8198             : 
+    8199      237944 :     if (i == active_tracker_idx) {
+    8200             : 
+    8201             :       try {
+    8202       39318 :         std::scoped_lock lock(mutex_tracker_list_);
+    8203             : 
+    8204             :         // active tracker => update and retrieve the command
+    8205       39318 :         tracker_command = tracker_list_[i]->update(uav_state, last_control_output);
+    8206             :       }
+    8207           0 :       catch (std::runtime_error& exrun) {
+    8208             : 
+    8209           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active tracker (%s)", _tracker_names_[active_tracker_idx].c_str());
+    8210           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8211           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active tracker");
+    8212             : 
+    8213           0 :         eland();
+    8214             :       }
+    8215             : 
+    8216             :     } else {
+    8217             : 
+    8218             :       try {
+    8219      198626 :         std::scoped_lock lock(mutex_tracker_list_);
+    8220             : 
+    8221             :         // nonactive tracker => just update without retrieving the command
+    8222      198626 :         tracker_list_[i]->update(uav_state, last_control_output);
+    8223             :       }
+    8224           0 :       catch (std::runtime_error& exrun) {
+    8225             : 
+    8226           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the tracker '%s'", _tracker_names_[i].c_str());
+    8227           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8228           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the tracker");
+    8229             : 
+    8230           0 :         eland();
+    8231             :       }
+    8232             :     }
+    8233             :   }
+    8234             : 
+    8235       39318 :   if (active_tracker_idx == _null_tracker_idx_) {
+    8236        8751 :     return;
+    8237             :   }
+    8238             : 
+    8239       30567 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8240             : 
+    8241       61134 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8242             : 
+    8243       30567 :     last_tracker_cmd_ = tracker_command;
+    8244             : 
+    8245             :     // | --------- fill in the potentially missing header --------- |
+    8246             : 
+    8247       30567 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8248           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8249             :     }
+    8250             : 
+    8251       30567 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8252           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8253             :     }
+    8254             : 
+    8255             :   } else {
+    8256             : 
+    8257           0 :     if (active_tracker_idx != _null_tracker_idx_) {
+    8258             : 
+    8259           0 :       if (active_tracker_idx == _ehover_tracker_idx_) {
+    8260             : 
+    8261           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the ehover tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8262             : 
+    8263           0 :         failsafe();
+    8264             : 
+    8265             :       } else {
+    8266             : 
+    8267           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8268             : 
+    8269           0 :         if (_tracker_error_action_ == ELAND_STR) {
+    8270           0 :           eland();
+    8271           0 :         } else if (_tracker_error_action_ == EHOVER_STR) {
+    8272           0 :           ehover();
+    8273             :         } else {
+    8274           0 :           failsafe();
+    8275             :         }
+    8276             :       }
+    8277             : 
+    8278             :     } else {
+    8279             : 
+    8280           0 :       std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8281             : 
+    8282           0 :       last_tracker_cmd_ = tracker_command;
+    8283             :     }
+    8284             :   }
+    8285             : }
+    8286             : 
+    8287             : //}
+    8288             : 
+    8289             : /* updateControllers() //{ */
+    8290             : 
+    8291       40713 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8292       81426 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8293       81426 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8294             : 
+    8295             :   // copy member variables
+    8296       40713 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8297       40713 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8298             : 
+    8299             :   // | ----------------- update the controllers ----------------- |
+    8300             : 
+    8301             :   // the trackers are not running
+    8302       40713 :   if (!last_tracker_cmd) {
+    8303             : 
+    8304        8751 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: tracker command is empty, giving controller just the uav_state");
+    8305             : 
+    8306             :     // give the controllers current uav state
+    8307             :     {
+    8308       17502 :       std::scoped_lock lock(mutex_controller_list_);
+    8309             : 
+    8310             :       // nonactive controller => just update without retrieving the command
+    8311       52506 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8312       43755 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8313             :       }
+    8314             :     }
+    8315             : 
+    8316        8751 :     return;
+    8317             :   }
+    8318             : 
+    8319       63924 :   Controller::ControlOutput control_output;
+    8320             : 
+    8321             :   // for each controller
+    8322      191772 :   for (size_t i = 0; i < controller_list_.size(); i++) {
+    8323             : 
+    8324      159810 :     if (i == active_controller_idx) {
+    8325             : 
+    8326             :       try {
+    8327       31962 :         std::scoped_lock lock(mutex_controller_list_);
+    8328             : 
+    8329             :         // active controller => update and retrieve the command
+    8330       31962 :         control_output = controller_list_[active_controller_idx]->updateActive(uav_state, last_tracker_cmd.value());
+    8331             :       }
+    8332           0 :       catch (std::runtime_error& exrun) {
+    8333             : 
+    8334           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active controller (%s)", _controller_names_[active_controller_idx].c_str());
+    8335           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8336             : 
+    8337           0 :         if (eland_triggered_) {
+    8338             : 
+    8339           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering failsafe due to an exception in the active controller (eland is already active)");
+    8340           0 :           failsafe();
+    8341             : 
+    8342             :         } else {
+    8343             : 
+    8344           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active controller");
+    8345           0 :           eland();
+    8346             :         }
+    8347             :       }
+    8348             : 
+    8349             :     } else {
+    8350             : 
+    8351             :       try {
+    8352      255696 :         std::scoped_lock lock(mutex_controller_list_);
+    8353             : 
+    8354             :         // nonactive controller => just update without retrieving the command
+    8355      127848 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8356             :       }
+    8357           0 :       catch (std::runtime_error& exrun) {
+    8358             : 
+    8359           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_[i].c_str());
+    8360           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8361           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland (somebody should notice this)");
+    8362             : 
+    8363           0 :         eland();
+    8364             :       }
+    8365             :     }
+    8366             :   }
+    8367             : 
+    8368             :   // normally, the active controller returns a valid command
+    8369       31962 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8370             : 
+    8371       31962 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8372             : 
+    8373             :     // but it can return an empty command, due to some critical internal error
+    8374             :     // which means we should trigger the failsafe landing
+    8375             :   } else {
+    8376             : 
+    8377             :     // only if the controller is still active, trigger escalating failsafe
+    8378             :     // if not active, we don't care, we should not ask the controller for
+    8379             :     // the result anyway -> this could mean a race condition occured
+    8380             :     // like it once happend during landing
+    8381           0 :     bool controller_status = false;
+    8382             : 
+    8383             :     {
+    8384           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8385             : 
+    8386           0 :       controller_status = controller_list_[active_controller_idx]->getStatus().active;
+    8387             :     }
+    8388             : 
+    8389           0 :     if (controller_status) {
+    8390             : 
+    8391           0 :       if (active_controller_idx_ == _failsafe_controller_idx_) {
+    8392             : 
+    8393           0 :         ROS_ERROR("[ControlManager]: failsafe controller returned empty command, disabling control");
+    8394             : 
+    8395           0 :         toggleOutput(false);
+    8396             : 
+    8397           0 :       } else if (active_controller_idx_ == _eland_controller_idx_) {
+    8398             : 
+    8399           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the emergency controller returned empty or invalid command");
+    8400             : 
+    8401           0 :         failsafe();
+    8402             : 
+    8403             :       } else {
+    8404             : 
+    8405           0 :         ROS_ERROR("[ControlManager]: triggering eland, the controller returned empty or invalid command");
+    8406             : 
+    8407           0 :         eland();
+    8408             :       }
+    8409             :     }
+    8410             :   }
+    8411             : }
+    8412             : 
+    8413             : //}
+    8414             : 
+    8415             : /* publish() //{ */
+    8416             : 
+    8417       40713 : void ControlManager::publish(void) {
+    8418       81426 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8419       81426 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8420             : 
+    8421             :   // copy member variables
+    8422       40713 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8423       40713 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8424       40713 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8425       40713 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8426       40713 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8427             : 
+    8428       40713 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8429             : 
+    8430             :   // --------------------------------------------------------------
+    8431             :   // |                 Publish the control command                |
+    8432             :   // --------------------------------------------------------------
+    8433             : 
+    8434       40713 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8435       40713 :   attitude_target.stamp = ros::Time::now();
+    8436             : 
+    8437       40713 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8438       40713 :   attitude_rate_target.stamp = ros::Time::now();
+    8439             : 
+    8440       40713 :   if (!output_enabled_) {
+    8441             : 
+    8442        5291 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8443             : 
+    8444       35422 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8445             : 
+    8446        3462 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8447             : 
+    8448             :     Controller::HwApiOutputVariant output =
+    8449        6924 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8450             : 
+    8451             :     {
+    8452        6924 :       std::scoped_lock lock(mutex_last_control_output_);
+    8453             : 
+    8454        3462 :       last_control_output_.control_output = output;
+    8455             :     }
+    8456             : 
+    8457        3462 :     control_output_publisher_.publish(output);
+    8458             : 
+    8459       31960 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8460             : 
+    8461           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8462             :                       _controller_names_[active_controller_idx].c_str());
+    8463             : 
+    8464             :     Controller::HwApiOutputVariant output =
+    8465           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8466             : 
+    8467           0 :     control_output_publisher_.publish(output);
+    8468             : 
+    8469       31960 :   } else if (last_control_output.control_output) {
+    8470             : 
+    8471       31960 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8472       31960 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8473             :     } else {
+    8474           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8475           0 :       return;
+    8476             :     }
+    8477             : 
+    8478             :   } else {
+    8479           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8480             :   }
+    8481             : 
+    8482             :   // | ----------- publish the controller diagnostics ----------- |
+    8483             : 
+    8484       40713 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8485             : 
+    8486             :   // | --------- publish the applied throttle and thrust -------- |
+    8487             : 
+    8488       40713 :   auto throttle = extractThrottle(last_control_output);
+    8489             : 
+    8490       40713 :   if (throttle) {
+    8491             : 
+    8492             :     {
+    8493       33006 :       std_msgs::Float64 msg;
+    8494       33006 :       msg.data = throttle.value();
+    8495       33006 :       ph_throttle_.publish(msg);
+    8496             :     }
+    8497             : 
+    8498       33006 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8499             : 
+    8500             :     {
+    8501       33006 :       std_msgs::Float64 msg;
+    8502       33006 :       msg.data = thrust;
+    8503       33006 :       ph_thrust_.publish(msg);
+    8504             :     }
+    8505             :   }
+    8506             : 
+    8507             :   // | ----------------- publish tracker command ---------------- |
+    8508             : 
+    8509       40713 :   if (last_tracker_cmd) {
+    8510       31960 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8511             :   }
+    8512             : 
+    8513             :   // | --------------- publish the odometry input --------------- |
+    8514             : 
+    8515       40713 :   if (last_control_output.control_output) {
+    8516             : 
+    8517       70948 :     mrs_msgs::EstimatorInput msg;
+    8518             : 
+    8519       35474 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8520       35474 :     msg.header.stamp    = ros::Time::now();
+    8521             : 
+    8522       35474 :     if (last_control_output.desired_unbiased_acceleration) {
+    8523       29340 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()[0];
+    8524       29340 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()[1];
+    8525       29340 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()[2];
+    8526             :     }
+    8527             : 
+    8528       35474 :     if (last_control_output.desired_heading_rate) {
+    8529       26916 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8530             :     }
+    8531             : 
+    8532       35474 :     if (last_control_output.desired_unbiased_acceleration) {
+    8533       29340 :       ph_mrs_odom_input_.publish(msg);
+    8534             :     }
+    8535             :   }
+    8536             : }
+    8537             : 
+    8538             : //}
+    8539             : 
+    8540             : /* deployParachute() //{ */
+    8541             : 
+    8542           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8543             :   // if not enabled, return false
+    8544           0 :   if (!_parachute_enabled_) {
+    8545             : 
+    8546           0 :     std::stringstream ss;
+    8547           0 :     ss << "can not deploy parachute, it is disabled";
+    8548           0 :     return std::tuple(false, ss.str());
+    8549             :   }
+    8550             : 
+    8551             :   // we can not disarm if the drone is not in offboard mode
+    8552             :   // this is super important!
+    8553           0 :   if (!isOffboard()) {
+    8554             : 
+    8555           0 :     std::stringstream ss;
+    8556           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8557           0 :     return std::tuple(false, ss.str());
+    8558             :   }
+    8559             : 
+    8560             :   // call the parachute service
+    8561           0 :   bool succ = parachuteSrv();
+    8562             : 
+    8563             :   // if the deployment was successful,
+    8564           0 :   if (succ) {
+    8565             : 
+    8566           0 :     arming(false);
+    8567             : 
+    8568           0 :     std::stringstream ss;
+    8569           0 :     ss << "parachute deployed";
+    8570             : 
+    8571           0 :     return std::tuple(true, ss.str());
+    8572             : 
+    8573             :   } else {
+    8574             : 
+    8575           0 :     std::stringstream ss;
+    8576           0 :     ss << "error during deployment of parachute";
+    8577             : 
+    8578           0 :     return std::tuple(false, ss.str());
+    8579             :   }
+    8580             : }
+    8581             : 
+    8582             : //}
+    8583             : 
+    8584             : /* velocityReferenceToReference() //{ */
+    8585             : 
+    8586           0 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8587           0 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8588           0 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8589           0 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8590             : 
+    8591           0 :   mrs_msgs::ReferenceStamped reference_out;
+    8592             : 
+    8593           0 :   reference_out.header = vel_reference.header;
+    8594             : 
+    8595           0 :   if (vel_reference.reference.use_heading) {
+    8596           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8597           0 :   } else if (vel_reference.reference.use_heading_rate) {
+    8598           0 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8599             :   } else {
+    8600           0 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8601             :   }
+    8602             : 
+    8603           0 :   if (vel_reference.reference.use_altitude) {
+    8604           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8605             :   } else {
+    8606             : 
+    8607           0 :     double stopping_time_z = 0;
+    8608             : 
+    8609           0 :     if (vel_reference.reference.velocity.x >= 0) {
+    8610           0 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8611             :     } else {
+    8612           0 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8613             :     }
+    8614             : 
+    8615           0 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8616             :   }
+    8617             : 
+    8618             :   {
+    8619           0 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8620           0 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8621             : 
+    8622           0 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8623           0 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8624             :   }
+    8625             : 
+    8626           0 :   return reference_out;
+    8627             : }
+    8628             : 
+    8629             : //}
+    8630             : 
+    8631             : /* publishControlReferenceOdom() //{ */
+    8632             : 
+    8633       40713 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8634             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8635       40713 :   if (!tracker_command || !control_output.control_output) {
+    8636        8753 :     return;
+    8637             :   }
+    8638             : 
+    8639       63920 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8640             : 
+    8641       63920 :   nav_msgs::Odometry msg;
+    8642             : 
+    8643       31960 :   msg.header = tracker_command->header;
+    8644             : 
+    8645       31960 :   if (tracker_command->use_position_horizontal) {
+    8646       31960 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8647       31960 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8648             :   } else {
+    8649           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8650           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8651             :   }
+    8652             : 
+    8653       31960 :   if (tracker_command->use_position_vertical) {
+    8654       31960 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8655             :   } else {
+    8656           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8657             :   }
+    8658             : 
+    8659             :   // transform the velocity in the reference to the child_frame
+    8660       31960 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8661             : 
+    8662       31960 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8663             : 
+    8664       63920 :     geometry_msgs::Vector3Stamped velocity;
+    8665       31960 :     velocity.header = tracker_command->header;
+    8666             : 
+    8667       31960 :     if (tracker_command->use_velocity_horizontal) {
+    8668       31960 :       velocity.vector.x = tracker_command->velocity.x;
+    8669       31960 :       velocity.vector.y = tracker_command->velocity.y;
+    8670             :     }
+    8671             : 
+    8672       31960 :     if (tracker_command->use_velocity_vertical) {
+    8673       31960 :       velocity.vector.z = tracker_command->velocity.z;
+    8674             :     }
+    8675             : 
+    8676       63920 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8677             : 
+    8678       31960 :     if (res) {
+    8679       31960 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8680       31960 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8681       31960 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8682             :     } else {
+    8683           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8684             :                          msg.child_frame_id.c_str());
+    8685             :     }
+    8686             :   }
+    8687             : 
+    8688             :   // fill in the orientation or heading
+    8689       31960 :   if (control_output.desired_orientation) {
+    8690       27921 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8691        4039 :   } else if (tracker_command->use_heading) {
+    8692        4039 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8693             :   }
+    8694             : 
+    8695             :   // fill in the attitude rate
+    8696       31960 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8697             : 
+    8698       21024 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8699             : 
+    8700       21024 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8701       21024 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8702       21024 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8703             :   }
+    8704             : 
+    8705       31960 :   ph_control_reference_odom_.publish(msg);
+    8706             : }
+    8707             : 
+    8708             : //}
+    8709             : 
+    8710             : /* initializeControlOutput() //{ */
+    8711             : 
+    8712          87 : void ControlManager::initializeControlOutput(void) {
+    8713          87 :   Controller::ControlOutput controller_output;
+    8714             : 
+    8715          87 :   controller_output.diagnostics.total_mass       = _uav_mass_;
+    8716          87 :   controller_output.diagnostics.mass_difference  = 0.0;
+    8717          87 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8718          87 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8719          87 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8720          87 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8721          87 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8722          87 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8723          87 :   controller_output.diagnostics.controller       = "none";
+    8724             : 
+    8725          87 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8726          87 : }
+    8727             : 
+    8728             : //}
+    8729             : 
+    8730             : }  // namespace control_manager
+    8731             : 
+    8732             : }  // namespace mrs_uav_managers
+    8733             : 
+    8734             : #include <pluginlib/class_list_macros.h>
+    8735          42 : 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..b180ccc2c3 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2204 @@ + + + + + + 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 + + +
+ 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..58da3759214ac6a671b0ae424d01d266222670dc GIT binary patch literal 25875 zcmV)sK$yRYP)}PfY%~4=f`U3pryJk)X$K>fNNK1QMU_O4}el&zUP`dwl0UT9^{Xh1{P9PK=yPAK$%x=piNO<|0^ z^9(VLz=bi68z#ayWT5-;u{zhRn2l5^GOqS9N;o-UhT0S|MwX8pMG|29IMo;>z-*i) zz_xL+7&z(^6WnUM@e12K9X-lKInj0ccM!%>Q3j{w-Qv=K%NG5x3Ezct2j8+E{V z-UfLdY5`1_X__=%t{^b9wavEM;;4;JE|` z*fUDt_7U8na(i+_pX;Y7HK4KUiJ%k#NA|tL%s{a&0S@mObzQ{1L)T{~k$&Bb!MNe8 z18zhsFpj7Cq?SL0JlL8_#7xcY9zjEvRiqwg)TLwa9Y!eBfEw29Cq2WQAIF)QSLf%- z&KVb%5^z#uybJ@>HfV1R^7ov9Wk2MO7;Kqi6wgUdDq@en_P(I6XT12%z-)cr7~x2Z z)-V9nW0c_&FY05|DSL-WK})4Wd%N*%{`qg&zVlyCSK8+AJNQ@2Ee90Z>6-p<@t;M6 z5@3L*Ju`p=W9$O}f{z@aJVpYD{*-o2iGi>u@`nZXXaV;+N`=)wLg_|rrZ|ghF}7&4 zDJZn`HE6Egy8gqOB2yDTADI=vwj$;^UsM!uTUi@mzETO`Yix7Ab|c^+Rv|cgjIjnx zf;(lqd{5J3ilSZn1GJ_$)l%HA;Ws6W5pcweq5;+-SX4$7CRvSfjFFo*1^*w=r!A`e zQfnbP5`M;F$^iwv9(<>zUZ)IWi3n8$C!#v=5wo#li>&f7Y(iB&hLxK0F%zL2W8NrW zoeT@r*z0+okRyFCA64;FB1U}TG>=Of;~1A0p>Er4x)7e6S><++*SG#o$sZ+wo#THy zfzQE2x_<-4=t@oB^2F#u)3(ov+Z;}I1J&9f66L+3rE9+wAS2I-6CFz-;blLO0pVSh zO`x_34r1~8#_7l@W7zg<&2>B@6y{OUF@m`c;tD<*p?ku9Fk+17@P9BtEn{Rpgj8RE z%FkL?h!s-ye&d8b+XiBSR+=Dd&gptLjI-g0sOUO%q_y@o$nykjhVv$S=SvQFIh})_ z&i#ec1`ObIcEp~&90R4>L`*sSf$W6SIq0gW{<$%Fx*?#>V-Ckyzq3El^L1e_^!ry` zGB{$CfQ7gn0?5PifIV^?mdP1xFGBnij6pO?l-M<$eBPD(&PMg?O6z$$p`u#du$6L-3$e#a z0fW6btgIg_(vTR~>wiRkT&P3ai7B+Si##AZF;YO0;RLWW zGn0uup8+(D(LOU%fjtn=wj4OWS?7KYu@RDWfJ|ls3z&_K0~FJ*GK_j+EL77G^bk|< z_pn`S0lBL?4p85<7Gnm=14_;-0;r$sz7z=L-YT3BxUPAO>=@O6%B5fd%=sf>6p5;~ z=ytv&l76q!BM;4N#2{ zpsWC1B76<7jXmzBv1xHBEr8yQJ!*h>AcuaCCVx-#WuHs(%kY{H_v6?P|H?c>Zj2Rk z1`SR!6!X%7!Q(XY; zmmbP+8Sq3Ge)qj=D<2EQoWbZ$B1YR7-3ZU&+C?~$w#@bUyoJN`2Mk<7#7|*}*%(!~ z_Ctn7xl-u+HV;Z}KJ!t(8N#{T}74XKz_hQG0*8hk>VnIY9 zffSJayFo#*VQYnA-nP4{BKAriMaz|LKP>@)p<`mWV}ilE7+ zu!n)0csP@pO38of1>|7Qf;JNX!{=}v78zavG%$R)k~(J_g#d__d&Z_-z!w)&YQIn4{);Yg15rH9$2+J>YaPB}K9>X+@A%bTG`{K(9(4r zf8};Be&ah5&RiGA_xhtYAfwH=&wKo&%)q%}_F?2Ne#(HI37z1K0z5W$ zjOgfE^k~K-z>rH9x?H1$(L+o)4JTv*AYfa4ah<^sBkz0sk711G5-!}MdW<6m8CV#3 znMTWW9S2(&C@24{6mfuUL_lHyA!iXG+vZGC)ZdGpQ{SgZH4}fZf^*%6O(P)&^2)($=O#3Pz-8l?^<%RXMo)q z)3{j5rmQBvxSak9hZM%_?QAI1&gq@FgU{C2snX#1ogpi zD$bMtLk3VXK5~o`lJ)}NNehS{BqqSqKogv?}{S zskRy%u}6`C5s>^v_r0{2@T5t3A>$lFW4hKlhEVS!AezLEv7xS+mK|p=LOWwHQ`e1d z9U0>e0W2qUVizT1qae9=N*FJAt_y(LqtqqeEERwy{di(+2{$5eU7&7E;BN2XjB8Vs z{Y8xD3{xcfA_21Sc7OOKSgK{dhJMbl(=#5y**G(CHqxB%U1y$@L^RY%=*J70_w>DG zj1bWf2it|Lnf_Ry3z3Qe7QiJFD+%y~&*38ukeS0@)wQS~)qsXEo`@C@aOTAHAMH;% zhO4Jg-!-gynQPMKljeB>hfMaMS@j?_q@0{nkhHtEo8phO>wHxsN2qmHbFZuanNB6` zEDnqT#D4*Vqulky4?e>VW(~ePU+c_Le zfCw1J;sKk82}f)wwv89v!6h^WV=6AApbm(YV(v!U^(E+yWWi7aiLTt{uDwAY>j8;Z z=dml!K=EU2=sGD%)>k5RO}cNSy4ZKtHFkN!uCu)`Q*5?o@By@L6IG0fil8u}+r)}- z^dONR7;M%>VsfErk{*g+gtssiGIood_orm*t>{}+bzNjk(~*?!$`Xe-KvuXUKqW@O zbWH?D8Se}jnE2rjpkHOAPqfRh>{6W_xDjwD7qg(9@3q0twX_?S%}^*l^k7C1sE#dj_P&8?uz2D(=;d84)%l~p;JsnyEnfjLfwF)zg%U9`)L4j# z9sPXA83_?1*LS{ko~}yWY?;DDVrh`l7@5bDcO2uq6f9S1jGXS-laIwgN_=GOo*l+0 zHqx~Sw`c*Yk9qS*%Yc}VTK0saJ$z>Xza1p8XJ%;T$2jZR!&tCgU4!mp7tMeR{2l4q z#PHw=PXV8i{ZqJ#FKgU1kWN5M*49kK5TE^o;~{zymdbxC5R+|(jPb#%j=pV}(>mQy~}XtE#c>;LN}kE*HunQPrt zTjsjR@3huc`}HC)JV$pMvLPWAEn89kFBH# zT$50|<^mW$WNO-HX2Czlsj1G)y1H%T*Uk)LxC*XGKjMj@OQwBh@WiAgQ=OSGF~4xl zmlK~bdDDGbcg zm|Tkn5Ud$B;1~&aO=lMRAq=A$0FESPQFeXPj#1AZ2RMcpwbdLt9;c)r;INsQRbAiI zrw?%O=^rCje|_!;Mm?Kvn@2hpd8&b_TVjv?%9rJ%2#7()Ed`5e)}61z0J_C23+VWT zfZ2r0faHcl1f&;o{eb#0W`No$jMW;OO@-lnq(K!rgMDeCnyrRmFp!2jh79Z4zAJ!h zz}~qgQy5)6VpyYgj6)9MI)*DT^`~{Zwbv&>_D7m+V&zx8Ow%7&-@NpgJb$o-Z+q-GwX` zh>-&K?RNNN?HYEEhQI;Bg|xDMTe@ue^*`00v510h_4iV#r<}b@MSpy!7u} z3pu2;kI^qG7O)NDB4(%UpQUlh{B3XAkO~o^eRCakUKx-B>$y&qQ}Q&yGq200_r#lF zi>JWHe2^_OQ%V_pHB)x#Yw^tsMLA>Un}cFKx*6O3O&C!HhI3(&z^?UUEC|R3`1&T! zoIFOY^yE^p0BdAHUGso}w{<`_k#?cDqJ9z# zh0F#QzdQ-Kvp~G*Ubp`_JP#Id4d5$}vEqI(Mj;bg@}T4bF^HCDnHT}%AYx9#Gm z3vtaf8A!dj1dU4+Zc7>hU3_!44RdEg7NfH3lzvsc*eIJl7ms4=TDe>s*yH)a9*i?U zeAHVC7_a;`R|ae)MwNA1S9{T0I6V2{s?0i7sOGrO>^R0+LT6=+32;w7mIkTdqpqx z<5&;AU*D)|*2<&R2XIblv9W&Q2D6_Jz&TT=OG zzW>o+AnRTz7hZsKo%);k+-ncGXb`e%2Hh;WBrR;&|JpUg-Xvmen zYY<|&6r$~X9fD$TJEL#2z97s3qpJSCubRaei+3ciYT_LhRK=$9ffK za1%(z$O6JrXJtSz(kdVbF0KIjPpW_oA^zA$65i)HgJ&}WCr9h%E2c)2BEd5Z`IGX7 ztBjn-0)qJPDn>ovqQ*F@0Aq-M8PFW;ehr@US971?0frcM1n5AsL~R%8(gL_}UHifx z6l2DlfHebLxG2dVtT{#waM_kC1Y9^`=8!Z#DeKl%kjeVr05uj(*BIGRier3`QU1m;uAdkq zU)HWL*9_nr_{i|GFha3~nn(T#iQ%&1&9Am9(_fO(saH@q8i{b3N3)TBv(1g2Y|NrnR;dWPR?J*iK;oz2C zz*vosG=tH2%8Fht3pu7NE{{SskidXSfC~_qtXH2iy1cLJynCi=xv(<<6d6v(*j*|* zTZOLNAd4}F<1wnDY-B#p8Q_f!Y-S%pdo z7q2wPeWUqG1B@{JMDn?W8=M@Yvg>ayfP-uTY$WEJ3ov$!-&}ycF@7o+z%a%_(f~*u z^nj&A+X(pfqydlSS0@elPY%Da6CJJ<+yUWF;BxT|v4wI7^qBA`7y^9=q3EAN5)QQc z6z&%u8gC4b6(0Isg@<2-SyB1^s+Zziqp5yxgfGqzImFBiituf!KHnbU%WKIoiwkPW zV@zR7gl~*>gJ=AA)!Pd`y^zz1=UOQ}f*``hPXOWbO1zlqy0$Sqa@top1mAyv;qxt? zkzNPT!S5?%B|MS7=wo;(s5mx@HuEE_NROI5QWip{q;@N1CEsRRi2-FDL*C%o|K!B6 zOCF(6u9o-AFX`EGvhww?13bY;IE5- zPz)-7qq|lEUbTP_WeM=A#CX*L>bpj;3WhuII^ZY9h{ah3Bx0|mns@K>#RFMDWn&dU z-xKEf9m!3&ZAR>oZ}=0&0C;0&;7T>cLL4Y8;Nmfg=R(?b70_dpJ?wCjuepP`!2-y_ zr1;ZhG!;s2X%`c>f_S$04SSR8VYtB9c*NN$m5e)s7pSh0zMFx>%`zT!Tnc5tCFyV( zFyt$^p34}Qq*7`%LFb13x{T9+5h09Uz-bLIF!#(n2(a2*~e+ zI%2jFj3K6E&@4>X+A%IlKt0tOjQW{LpaU5`ULKVgZN#Vn`IIic(6|=xwrBy>7|T`s z!4SKu_!Ik4RFc#VuL1IfT&2$I{knWD0KIVh?Ec#4ycMMX>+4c5 zW>V8dA-(&!ani`LJ*y?ak1kh0sr$w;pp)S@Mn%&5LPyVx31I+BS!_EnT-+?{#4NY$ zc0a!*mB{&R#4w=>zmb?>V)jxy!WiEIqnU}8us1tL`2gG)*Y=V!J0aV-J>b8p=?ycPO zAl!_N8?P*HU}-nmJw=wf|kDphhVqqDKR{lu2-?wbYGZ$Df=K6 z2*~=uJtJ<4t~WT?wdnPvfZHeHaS0pj0!HV@)iXT0m-t8J0mI1=LBO~Jpq`kf19o`A z1AMgO0oxc+xfEn#@$89!tW1Ewi<>WndKJY{4R~|Kr#}d&6bktu9%Fa6Zy?4D+=Fia z|1A;c?*8Z#5$EL%Nj)G`-9KLjS9g>Kcq^=Yc3%f2@lC6pq8B^~Cl=FNVlvE`H6E0W4MHX<6(ipLC zM1NM6$3S(-`57qd*%%rlvYia{GVl0#x z@ty2dVnmBEzm1RK2Xnge?!k!m&qWIu!JE3COJ~x)y$~}igxMMZ2X*ZUGy&?m-gL#Z zxd>z3e)nANIlC52uX4Y>5j91q!dQt-?GrPhh>nHo%ceuO`?x9G%k|t+2aNHG&j-h2 zlrLho3#8>mM5Zue6(AFPXfg?1T?+#k@|H6;<`_A{ct=1z)ibYkVO@J0#%+*t8M{`D zcywgE%Wi<0F}~VswO7S^GqPi<aRH+Pq_-&|Ae`|r7@Gl``S`;+9LJXSOvY(P|KOpinV5}>tf=v-O z#CDrPXj;J8Qh^0vluHq{mvL zK=5gFJ=0B7cqJBfeRb5h(2dzY<|eZTdSw0g+Zzgr9|;hC8YX~Z)VLt^;=R$ri3{mw zCqPXrNO-QA1tmTs7o&ht=(=-^xV->XPGqZ+dZ{UT^YFhYsR3QvVqL%b@v9%XDI5-{ zXq%j!x7~_6ZsB)0x&VrjeL(W^0yif0b{d783P;9uH@$SvH zUimVTn4%3V(vxAi^{SMc3reU1nlNsITS_I#o2Zt<*KSSm6tDET!W|o8cm+_|b*h9` zJz9@{^fz}5^$~+H@=^O4pp*vy0sS8*KrRD<09M8*c^>oZf`tWaaK-z{9#fl!nL^I# zNr@Q=sHb|bdPF0fQLl!n=KQU{hU&WTTvO;}d5pUN>ezG5x2{f%dog^Jh`n0k<5EhF zy|n=TPcxIoC@XqG%s@aL)d*w3F8(l}s$rohMCJ9}R0x2hQ+IY!6jKcMDgl0txUUgc zoWidW*V?Xs$%s?8>(|;F)%LLVma>oH=HJ~zhTf16tsAr-OBlxkTE_Uhh2r`(G{1%> z1vIqLI!d@KV2JruNmaPi&(V^qfE2P4Y0g8+{+in#I&J|1<_u6P6u&-x^>ma@qgmyntlUXE&dIA!vc4}wV*ewsjRLI_oevwRK-x$%o@$Vy@C zp0v0=xX*mH2&0z;9pJSN_&N3j@o}Qy6WzPiOp3-S1p*rx&T0xOoK0SZqEy&0YEG{k zMpm0JT@MZ!ypp3P(Z6vx7QoR?7h$e5J@lRHVRQZ2s83%*V=A2tqnlD9ClcfSy#|{Q zoek$?aof(W!4C!;Vs0CSBS&$PS=;0{35B-nP=n+Wjf6OmMpFU3RT&J20p}UqvN}DM zTL8fE#Oy#f=sK4QmiH0xDgl}c)Tr0Ob-3<1X+!;d7ytku07*naR3-A7%oL$5JzeMJ za46ODP4uIrnei49^`p#~N1s$u$=}1rAcO%Yz+%Xk@io?>gB|(oLr_CHz@1~souieC zhO7g8L`HxuFD~=n3Yx-zaW0|SKAf$YB%B9>15{%q=hr!k=bRVbYBuA5sGsjQo=iWF`=3F({xsxgYXd5Jn2VnlG5?Tb}4Z^$%b)GV%7`;>0}tPbZ? zcsRA}`%-p7d5*P}6TngnN7!6D4n6>3__EonO&IHdpE}Q|mqIRP?3(zBSskOohU3rIH^Q(j2+=Tw zSB(w#_s-YJ?wLwKE}iB3Pc`xhHHqqE6)Ip27BQQU^LhjX8;?<@)=-_{vO}* zGXTPKIBJOTM@aYbHDw6YuG{ z3B0p?bIVoR_4YNRB}bV7u3K%(`2Co!pD?yRJad;YKK$tD-tPNXE$Lj+q66v7=WQzs z=_wnfI$11%_)V)n2p{3>;O53SZh*e!3pNCPlcgJCLp_9ua4Z{hXX?foLCh3#YjS<+ z8)l;nH<&xFYp24>P*>u2kSYTZz(srldxxk`7+F-@J^ap9N{NZYXvgSgFxeXyq&LE{ z$RksitvEF9e6i#L+~Wx;*#0Zj7V%rtJ_Gi&$w&1w0& znduzvaNCX$48QlwmL67jh6^MJa|}J3hJ(+q&uyN6O>VQC+T4^k3~B6Q_;4RUIB3?A zr8=qRrqF2$Ce@R))6348xg&Zm6vep%U9ZTMD0i(Mqw1M~JJF9g+U$}3eS9{pngsZU z5*i&sv4cvXLXi@UNgAtKaFy$om|_J<%E{Nrfl&Ap^#&1qS1; zIcH>Q{Ehg_xG*K!G(U2{aVM)M6}iKt1Mi*MxUe^FNqT$`-c1aGZk?GMc=iE(A?ztp zt>Vo!Weo*aetC@O{4pd0f+0IAfCmAq7;Vju zJZ2a}l1nz%sxhA2!5O$lU1KKk0GFgY7VsKX!7D$KrcYo6ueKv;F_YxOCnT2S8Oywd z>bPDaPX3S!SM+C2!*WF~s~?xv<&yzz7&U+fj34y@u;)Ve!RkN$@KW%;f}AI<4Ck-& z&Fl#r`n0AHGGkFY$yT`Ev={feZCz^)NLiqya6oD)Tb6K0fNG4LX9Wi14rc{v4&h14 zGKIr0AeC&qo|H$f>waZ#?Sd->z8Vum_$gfAp{oI774UCXiv1dzz1xSEo_x6?2?wiS zj9CcFwF;g+V13Nq3s)$HkHkOs=_u^PZVPW#dbsM(=GfL*gxIep^6g zMK2WC?%@M$x`^QiC<1cC>=Kze#YiXTn4Tu?W?e{(%><}D!Z(C_=c+WRwr_|xmHZ}VKv1gUN)`(unps|xz+&gG}q_qvgU6f^QmrL*V3xC#S8lB8tZvzv= zmisBhDO8Rzk2ep~i-Q=L06!kGEuZUWv*P01&BTB)zR&@Ag>za0L;=`Ej1V+q&x)4{ z2DB6N^s+gQJpp5Hb^)6F$I6GP^u&>py{#4F<255yK>HY9KfLC*3Wx-u45-2A8S(=- zkQn&{fB_#5S@Z7-xZbOPn#OCh?nE26rhcZRurC@hzLwVy$*YT@9)K}%Tu_eBprjZ; znw=tJB%lybQjGx6OiW=8P5G|cZCEcozdqued4Gl+f2`G?)OL?#QMmIfoofvr!y3Hb zi;7}Tz-L*R8HoWF=>UxC8`^wk)_m=*eTL?PHTi7zBOTv{H(X}A|5k)CH+D4`L-OA0 z&|4Tj`rhS^1X5T(&*yVbz-KvVx#bA3l|U#0-n~jF*eKXkr~(eo2K*B9`-v&jlg7A) zgWem78NIW^<8^5atr|Cx?>IIHW16Lb81I>9^KF1ZRjvrIlRZ9x-j4C){fcMrUYjIC zfOHMieLhB0*Q5*rlT%K*<_i|wEsN_8P@;`%WdO|>8R4K9Wzf)eQhSl=f&tkrk1Lgn zHjLu!L&q3VTFe@flEHPV0%KY;K8!?)WxwZwD|%=COTqoWBc(?@b)3~zdjdZ?Fm4P& z0oF-HdTJo{uA}HhVwkhVngE}1KVT0bZB$nbBrVm$FT`+tx?exWVv_1F8Dm~z2b6dE z$?BWl(J7>R&X?TMxP_5*Sfjzs%77nVcz^LZrM|vbZ^Oscfa~IW|+BjkoCm#<| zXiNYEbmXp22>2{v)Q>Ua+6ub}$c_GRFn$eSk>zb&R{$*-Kc|`tMGaFU_q`$qOoE%- zI8;1>kl2JG`+NQ#IRkus$c3@x7@xVScpKnKl+S8pkJ3wV(jPx`XSl}2^(;7ND4^&{ zY3^EXO&BZpTsxSBq{(B{cKtb_pl*s-H7qYP+AKXhgMN|ph$3Pr38t4HW!uGcZ8$2p zI?L#TT_36nw)LpGWlZW$@8c!EplsO|V?pUklf0zr?GfcoaC?C$G_Ey)knof=UF1+&eSNt%5saGZ<;2s7}D7%O>(jZcl# zsLrSfl|%58(Ho$uYi&)J9Uk3443{{{q}mqPqar3@R6NmZ#8}4L`bN%(F`kfZ@2ji& zw;RdKcjoiX)5fV{(j5}o4dZE@7zD0>uT{e z|L@mVj$?8lkPaghpHTYZ4CJRleEJ-fJ;ab)0CdMiF{-5Eq_14O@4KXzGkvD3VP?oR z-~)e?LRIiyh|8A2IuIYN>W10ly&vZu_MAd8ysyqV;>m`yuM&~M0i0>&i&vWmd4J+b z8<40CW;z6?^L7t_OXB*TpBULQ~DT;acq)>odEr> zBnz85DoN~K%NS8P+awr0k!jJ16bq;Ee|e{?RX8u?fS7W@lJbbw94-e$Ob5XG0P1Gu z?n*U?N%1X#yW7T9ee{dlhS^oI51?3-rZh(6sd$8!$sUwFl$i`hMMcTamQ6>Cu|U9!6gZVBO2X77hR4 zc0319)|3HG=$yc_M`JR#v8TeKHjLRy0mJ`uQJ;fveE7&idV{}MA`7$;mD|<1;#T|+ z@sBuAAv|u=^Qh$qZ+rmz08t#(YW9@MD=#>1PbC$GM>h^|Nh^#uls9m zd_bB0&KsYYk7NF8ruoNoyo_+Kx&$$EfJ*|DVrOnNURe}eT>}`?s|nou3C4GPg72G4 z3`y+qq--(Lr7^0Bxk3^UK^s$poVTeIpuDr#CKLc&e4XQiDh1L4rZ{80wFo0_ehr?_ z=VT}v#+alc8|?PUY#rkj$M-=&2%jvrw|uYU;bu54+V$`;?)_ry9JE0*F^p6sy&P`H z&K9A#OVxIm0yR7!K6yS0fNG2mz>lNwP__engfnLy(3d)|0frBU5Tj*J&hQV0#O&q| zPe#RvD3bHYF#$8YKFi(GROFOyd8HexOHJa%f3_+duF!JlwZ!zt}OK5>M zTiH`#Q5(i=rGVl8)d!yUtP(>%_}*z6x@N=>(s)veqd@`eX;O^x_$ZPkUzvZV9H0L& zSyEj(=nnmz!c#R5KSuCNA+%|2PUw6@ygkL*9MzLISK}kaZWlvTKj^?84Qu93kV<`1LT_e)>3Sjoq)naLqvoGaZ_AV$L z@lkM*Iyc)R`Nf(sf~E$1HhSW+SS~@D_KPg=j&*MtZl9RN3yN7tBrT~Q{+ZnT*;*4H zaXAN(k9v$^H3{T-ZN<1s&ywa{C!L!C4iDyB$ubo<*|%IF8`lKbf^p5C4wAYqycxe| z*p64=X?<-Uw(0P2$5wK&@QW*dOt^hy}aYzb?MC|%`k3hF?sAzTXt%^z3NW#i_yJ>hsjYWN6i9I zvIoBmB8G~zM#Y0bpm-#AG3Q7g@kx^Ih59yHH(yh2{wuShm#{J*Ad{$w0kt!eHsTr} zeHet@KrOZ6^)0P{(hc_Ys(aDGSRp@Cx{=ROlud$*d>TU>8hAB;oTaoRo|c-@7-Q)m z-a5u7e`Y6uV;)2kx+dK-fS~S0lViv=g`&RWzC}G{?K-?ycS+^Vq^@87q1O|uOneRQ z`dMiKuwFVy%^r0_Bb47_wDNJNzR$P_Qw;raZjF2#YjRyt`fm1APEeUvZC0h_`5Tg0 zg7G9mL0!MZZC2P7)a*fv!>_Ifxi4tUfEZNhN28btU}0_bcb#>g0rHC3x~{baycCMr ztWrS14Y4es>$;|x*`14TdX=JPkGg>o)*i}MXXNNx*#6Eb5{YNJ4|ewf@6ss=Xo^^% zfmHynFO2m_+b%CG?Oc|}IrY#7j8nR3jd!J0le}uz!I%SN$XI^4RS$E3lyY$(o0@kn zdZdWoT9y4H{5e~+Gy>`|I)rsVwrVO9nL6A``9GhF%AaEDs^0~&3&uQ1B4iglqO8a5 z)fB^?P!>wnw&W{h(M0GYi}8^Xx+ge6F=EQ?d8Zh;i*B961hz0-Ox=q*!TrV@FrX+> ztBnIt!Wo2O-^CS=HNB2uIL8Clmk?P1_v;O0ubYbEMXGfQ;oiCwF2`G*cg)HP(J{J` zNHI1#mOem*L+n8|>snbk2(W_D)-}?%kE~>g8DqI1um>C-6Biuup%N)*jY4ttc5H^C z1X!uJXXv_m9ezDrBI4yTCK<7=&0K`aSDP8NS*q`|LM`ysW);A9YO}`s>#sH&)5hCo z?7;_0vH!Q_n4(Fm9;(fN>M5@5CSu?bD|0az*Y&!2lh%?M*QoXibiY^aHHXiCOzoXw z{&YA?Nq~KiJN4wThbaua&KGP8#s~BK2|;Mi2CSCv2_dYj4_IAeO2R7@apM>UtjpO+ z`9w~o>sVt@=o+NEqpo%A8Sk@GuKzm#99H!1samZDUy8czy1NdF(v){s{P{t4*Io8P zkMDa{pdB1y(-iN8Hs^VYY_L1d)4fo4Nm($}g8(gE|0Vr6(2w$Jh0_Q6#o_d0$q3n5 zKZZ=8N4U6ddj0aNAB%wMCf}PoB^NYgkACcq$NlQZA&uOP4p5SR%sa!@o$0|?iAo8Y z(0UritBYe+qaqyysO)-t+Msnq3Uk1zZzC~=eFpHc@@#^s>s-vpQrcW#;gU_&{Ix{fJNj;`-9rQG(uL>}SD`C|conISR9 z{TB?z_7JS9Y_$ZF9%ITmFW@w=$Nshpw2|0K4TN{JNW>xlX1s3z z)bf!}(#RvP?qNrYUiIfAd=M{0R(Mret{n8ytIAN72fn(yPE={_t^|<}MYg zGVR(og)fckwsW9zW_LyvzpaB z$GJbK)r{m7j8RWaL^jNH-67cB-@E!JNY}VU_}pQ44RO?aSG>WrVf<7KyF0IG3LKi| z8k%koU)Y4QM?Ev68NSapOnk`sqBil-QkheW@yeCg;{i1rc2>+X-+--S96vSpV2FEJ zd;u!VB>|muVNZ|m1?{NZZ z){II!c9lY9)#(4cT*oG-s+Q}kwJq05>GHVENM*yqa-Eq0=BExl! zyQyGhvH-Xcz~K*$!wqQ`){E;p0*>5}{?hhALryy})&fpLJ4h6X4?$pPTEPxw4M7Mv zhs33D+>PVy!dMP;v3M22^S`im@WkZD-gzpr{Tl5H^|zQ&iI5Vm(OLidV@LzUGgX zOZZE{oYB|&4J6M1)uET;&kCo|ZFSPMRXX|gpEtcFb>E0nm1;@MP=(LOLayBzy;BSe>4(rW)|^A@lOHM*yy9Nxs+Zmq`+m@?%m7t@U*r!Nu*k!6A1@ z!)P~71+W3*Gdr;IfUn>FoPSgp$0Wuoct`-t9QuDVat2#*B|k1SAKHw#n%eu^78gkz z<@&U^NPxeN_xtNMe>nV`sakvT2&SMr)5PuJZp{QiDs3h?W%AfR=}9e!-1+l*$1ZE4a*nmahMz z;0_Ajt%q_r2|lRc#@3GUE4Tw1BeQzauvaIk3S%| z%}0Q*;0|fI;P#rjHNl0BAuYpypWyaj{0i=X_6=7q0(OVl((qU#)upgl`&q#cvUcYa zU3M`sMEC;)sJehqj8WJ2E2JB9rGx-qtLyVaxl+zOS%*7FosTy^u@>b1e^64nD|DQa z3d+x_q+(854KSG#(jP!T-OMP)sB3#Gsr4C(uw|5gl~kqY-7Ts9#Nn?cm6ml{=`9H& z6-#P0^U)3RtEBjbEh)*4`YNev&j+Y8th&;897IV4-7_%KC2?U;_){qLxl#j;=$234 zUw`m2{;zsx-gEHT?8-weSHHE7#5jDpKEdm_+=~4!*D~PImg|^M7kRnj>z_XtGdwu) z%O>H;@d^5+fR?WRq=b(V^L-DwbqNO!-$NJ2;$QLudLkWix!=Z=s2Gn)&aP4^%Y(4T zoOJp^&!&hwTGO3oViC{#H(oa7d_<$ULw#BRB#O7yGhB4J#9tWK_2`-|%!thNhU&y;G=@j`Y~8L4Aj9d)Dmlhq&T#dHr0n|6&7lJDPIa0DnCrB4 zM?gK*ten9QA08v@2Ll2*i-6%e#d#}pK|_FwT&cR8ojzP_11hsstiTch6;|`#wQ;+R z+;0KcF>}p7&jns&@!x!HT-}>?FA3qw&-%yYz{7HKo!Z4FxM~}heOK<5R|&;{3wl(p z07#w#BX12UiP_l0Jq$<&ZCuyY2QnHS$s#-DU$=23=?SwHINbJ74Vh*-Gf=MZ80PTV zvb}%pK-@c=v-@~<4_iXnMQ>kKgyJ^%6(|G4l^c?-uUWk#2<%>^Q7{1*|*DCXNFuVZRhEeo$6F}hS&e=nR zBI4{GVG08GeSiP2pbQ`KKJd=MWF; zEVLgZ3PfUxv)rK^h4qw3#e>A z7`O0*W%)sfW@N^KTzcvl4PmDjjQ{cl?Vffx*fqbKrn2h_oJ6)e6 zstdeYaT}gfh}Cx+F1OEa!)VSN@+#m9jw5y$Y;*J>^U9?fcgQ#rziH7fpVH3!bU&uSi=*Z@O03$LtaQ+Vv{qwn;= zjuFAVKC+)ap70O)0c*sN(??>!_w-Q)JlyGHh`YJAeT4L&@Agq{-|gf1-9E~Iua7>y zeN+L%_K_2#Y~Ssp45)5@ve12?c?`Pp%;2LVy$2bj)#_?`g=h@7?y0-@RtoQVmORzN7YAW0pY}wMQ z%U*wkqB$%02c#^Mbq!WDgp3;3|I+CIb}78AI^Ima2~$gr^1yE15zD^t~K9BVo%5A@dfT^Lui;oAai;tnY$xh|pB;K@$nbYBC^p#!W^fLaI z_v*VYN(7Qe?HILu1Ti6u6t=LZ5@iwz+AvB0u(d6Pncdm##9S$B7;fDxGc){=#XH!C zAL%VPxZQ5QF0KKrBvae#uz$S1`Q#ids?7iZAOJ~3K~%av$kU0yQ8*@60nP{SYE(!}UJ>dtOM48dp~eHLZk~ zM6c8ecvT5q*Zf>KLgSXNGSC=<%#{ImaSTOUy(oKjwFmfERQBj0!4e?12=@k5GF+-h z`XHoRmHp^?q`?>t+i`f<0MecB3c^~j>;s*eEK7~=r_kVRq!0Dg3fbIUapp_Tv} zm#cX16t7dap9%{WVf+D<+0zP0ngE#9HU)aWcU{v}{ax3!G4`(O>`_JmaLrWkrdlaDWB7caz=?So-Ee?2hmL2XC3xOVcyxkJM1+w*p6=n~6-=7s3IojrIT zAB~b&ZZjKp(h&aih<5t+BcpqM|IBP<9p`yo@)ff{0kvFohpDM}ajtH?^3ej|L0@wm zKsDISGS}gHFI+S6J58Vaft&2l9_{zpkwW<6Wm~hU%HX-BVAlAEArzG&JtCaPa>12D z%D))|H*4Iw=!*_p!bdX#%=%6QEeKFoPr}4-QTfdC(j2xEsHO9P0UI#pM#Zu(pnZ&) z*v!Z_Yr)75F97Z!6t%Wk*+c*S^@XRp3FFM7Yr-h%lD;v@5qa$@AE&=W&;T|n_P)Q62KQCg%pA4{|_8Kt`1f*gfW-0;d+RodD zys8^8GVAXhfTl6ZsQNJO#m6}5`vaQBsNv%qHcfGiT4FMcEb<+y_>Vt*hynR=`WGvE zA`;%y1o)`N2rC@U158(NyVrEU;`q2j>P}j8^6r5EUo01UH%hqZe?W;}YCwJ0+0HdH zEVb#L{Jh7SGfIrFmb`W$kaNl(1i0U&`#txdI&_iftiA24h_C<%E(_uf`s;fFxUT$V zfybz^N%d>yzh%!p@nPPt=)aj5@8XqB2jnxGpu!f*f)b3G!f!mWbJ~O+R7{Ra#Z=Qf zO?j$K_k|%A$lo;Umcn+&-`VvYQnSioc5N6RKTVSM9W({tWA$ca^bPIS^%kS=rqD(0 zZ+9y0U#I+C?jCqKYZmUXZ2L^`4!ds8@mn-LH#0$eURS~0arPc%$YuAjNgzz=i2$*( zO=~Us_Y+{nzi(Syes0cZFg0%wi@=^IZF$gFAX~+7MPh?>j5IAo>bl-MUUyM3rAteA zYv~F%M4}&sIFgmA^d%Vs<}t=Mo5V>Ee<6&nERO}ijp`T$^a{C(sYGUmdyMO*Z{r+3 zviuS&#(Jn73wUSJrdU8O49X0sCx!`{Svf#8MjB3D1k_=ycJ13hHI);?v)v3>k74(; z3?d{EgQl9@ki;pJa??Z8XCiZV>|ERBVd4~+s*!Lf%^^}cMt~-Da0m%k1~r-DaaDfv z-r8Ze{Q8XCU(*M&78xU@xNVG5Q9$_as5O+MUKFr7P%=-@OoBT;P*N?pySUAw&Yc+5 z8A1`8FypCGp-K*2oqPbo@1Ad<}A$3xd-G2spmLFvlE_ zX<6W|1ekZNqk7l0L2pwqXMxB1%x#K7+MrI6I*ePvg=!hS09bU!g)wR|;wCu(b9V5> zh(?FmvYKLIN|X7potM2nLw!N5cH`AK5`TWtm@o=!p%){sAv27^LIlm&wL+iSA`2w% z6^Z7S0jc2X%asQdSL~+azb+7AcUS zSGfQ7QIf|@u%v@{E`q$3qB;6(+|hDzFr2#K;=!BkdJA9Rua5FW@7M=Km>1rKOcEt` z7`LKu?-I{kwH3R+J8TJ*(}iJ*8Afz3_@@MTATb;IP))Y~vKMkThI>Tp^Xfeq@m!Vx zfzZ&YJ<>@NT++#1^Ep1mfGnJ6aNEJbre0iX_Uu{^WYGFztK;{@uxqmm+>uM6ns?OU zmDdiya*gSD-FF{R^g-hQ%^2ZW8MI9p;T1`x>sfrUrHdFcHTDNqBy0->Y{9tjyf$L& z)!`X~Iy`a0rnF0j*g5JmACj4hqi-?G23)TFhIdnLX#p}U>zEl%^QZxDyWEI_DGW0h zea)YlLaxq78^c9+ZjYOrk6JJQ?l*-Kh_$+aujVO~6S`3?qU5s=3@2wO^aLfx4T}Av z_M@HQzUWoBp9za*Aj5O>8u~L}0&OO^RX#3Sz+c4h!1l`pe$c{ji6MlTBc0gCoh$f5 z#VxBS*kE#f&|uFRjebzi^~X=_Qk6Z_wfe-a?Rv|9@>@)J1}CK^PpNC+)`g{7oE@nf*QW-T5jolJj%t+t!wW1 z@t>}14kHiPHO4OcQQh@;c~m#LA@$3zIMw-v#7lIjIvtC@n5Z#5SMR!$Hc(T2Tz0wU zpPhWk8wJK4QYC!&id+K8cdcxYkK|4F4vdVfbXD6)XUnN*VaspE`#~&l3YkU)$?B6@ zW(${;=Gad`gE(qkv$=?ppsC&X676AXG?iuDm;+`(^)>A0+ zup0NmfM7+NsjES=q7-^PMl!lk5(VdM8ZwbV(8K2X6!~L(C2x(^XR4?5Dq|SSU%db~ z7CiIkhMVh2#q5b1VE)$W>PvPcDS)Zp6>H_QZ{)-&JQq*ws*;!YRMLT)LNtxPa|+k^ z)AIoz=sUy|mS+%5VQ~g)QYxC0^ z5;GZD>e5c3Zfw!J_E6wmir&$q6xNLHdu!d9p=U1jT&D^V3!Yudp7B>~y+T2qx?iCf zEfi&$*m2V=x0#!3?l$ZC-CbGk!8N1ZH1A5U%ON(TBW%OpO|#7KU!Whqn0X75OonOPWfgAk0{XF^ zojyU;G<=K>%HALH63aD{++K{QSV|x3#hl?voss~rwQU%@3{t1s3Mk8WAH#W7VZ*3V z74-EYQB_zTCDJY~JlBDGj+l=Xi693^P@_A`P9>^BJ}cOR5DP-~2ZIoQy|rh)fXG&L z^AxI^(ll$!PA!>c5wK=)H3P~k%owg*H_faH|t?;_iF+S?}Y#3dn?#ezlm|hq( z$Kys6Qe190h}Yj69)A!gFQ@a*K;JD?yCdo3k8x8}r(D;FJ!C@T&!&f9+4vB#TT`yx@X!-1eY#?6=T#qY+J$j#DHoES3N&d>KQU% z2glHz-WLkF?;p-hiF<{??!J}IDh zMzU;iAA6MyT^GjO<+Jy3gjXC-jPA*fXYmDNHN#8nj*ltZl+;6%1PrLgNaz+2TgA+M z^)2)DM5-f4fS{3Pv+PfxmMZ^_@2jEFJ&_&PoQaGWiDU<=m-gcxH@*n?p;O#F(`LLX z?UJ7G&}>zD*aLRI^kmi{9^I3OEjiEVsfJ_6kN&RE>7vKT?_6sRI9hr}D{)VW=#{}# z>(~RC%`t`p3~sP-T?@0)jIsFAa_<Zff$oVP#`xrhKI#Gu*db7(V{3bEOUk zV?=CfXZR_`#D#y-GfVVd!@miH+_{(VQSdl8K=5GN1kj%I?OjW?boU6jVYb7Ich@#P z-tYC}Ui1t<6eySxfPD3ZhozKuIwT&pa&`4~ZK9b>wO6W@X*V=P`(3U|4W z{PZEqY-&5q^@(iQu?Q7oUQ?>YLxqMG^L6SZdxV)W%1R(GTr51yGcyk8%C`*r$ zkDn}C77k^>k$h%@rNeejy*KoO;clD`n?f=Mj2_MVz2xaMP^3ga?P=>cSg&a1#b9z8E z!<`8?!Q(1R42^zReaYE;jM}cT)>}o&O~gzW(9Tmlt>YkY#>vIHLa1uaEV&^2`#lUY zg=FRMO*TE_qmY5$qddfyV$RIA@WE>+zzL}9dRq$Ix_N1TGBcTF&3mtRQhRl@2T^+8u*nqZmoCm+vpZ9% z(~7#}jGnmc);AXulVeDgT%d3FG&O5!)i~eY8qd8s<5&0VX7)T-VZ58Jr9T)Qx>u!! zK~F`!8Wur0B>1>GJUV6)f_Lfg?LWMM*iP)}9P=ayoo<(dTJTU-S9Qcdo>P6>1{m80*SxFNa1(B0olL*W>fKf}A~` z{8eG}#Hq*Va!x`#&J-OWxhf9fJ<$*6y>a;%AFQM{^YL!oU86O)CcAKrzGn80otNBp zD@79BkKWo_Y}}%l`E}ALW+_69i?#7$b_O?R7bYw4cZ!*}-`s0v7BTx|il~BSw;`pC zxyT-(k`nQL9|L^UQLT)qHe!6B0SbV}@LrGhQ$SB0tF*jS=2Nm=H1wiNN!sk6++eT46`QdWP- z#=WYpAKDmRHk}*Uv!+z<^_d^K1 z^r*X@D?5x;DCmu^G|+a8L&iAAz>8v~SkiS`+4BtVijBm)G{hbZA9N!={MO|}*T3If zI8C8i|5htSOyYomI~Z}uVh831VfZo1-iG|6Q5X?pvKNL@CG5sHDe)|ITkx5yaOlDq zr9;<34rkIa&Oa@yoNL?5yQ&lWzQ>e8?Q-TQ#T>5pALED5uF?gKMeR{OevhP-kDob` zs+WR^J)CpDi#-lu9Z-$YdISJ<;mBoAhB2)M?2m35W8H~-sJIj-(9xLkT=YM0H{u64 zN|EPK&}*95d#znFFUx1_Ft1_+bnCjlUYX#_*ZUFS2j5Q~Ju5WR!$~^sTxGJusEt&= zT1;d=hjJ3kXkgb-7I3ezYn9>LG}py5T4SViX^b33DNQVv z%q*BiQ*4{&indHPY=I{4uJA9sskw$K{)3Zul|!VK6d7VN9cc6e0(_=S&(9XJfWDNJ zt-!pec^qHLWuQX^+jwjs(8CEun&w)-sjlHCgz{eFFKx$^)tRxupna;mg;E5|A+CH4 zPci9<2_fw1)WsjA{uCc`Qli7Dtq3DLI|zhq*KG^P822kq0C6+7D zN7r`)AHkp_uAYpXXHBMglAFZlet3kK!>2P39zd@ZJRgJa*dOuhy8SOI28-w8 zR0(H_ZI4d%g8>bo#C7!*z=k5Ns`ovDP#Ow1UKn>qL2AmN?w zKn~Ej8P#72FA;M@33qqDK9ExEdX$7q*4~Eyg?By9T-Pru?d(wjem=(sPGJy^HRVFI zoREk}5bblE{25UEsXviMU^o6a@G*lVM{F-W4Kp+9q`8lNV__*? z7w`uy0=PztsDPAWEIwYsV9Ys?jj>eVa8;{jHO4BsyG8e$eutP1^iz~)FJb%&MXCJ; zp%~~aAk!zlEcu)-qyd!gBGoBfpKlF&rV{aaAZz#j3EeFuleK?=&HHYCgji=d$P9`~=> z12WB^{Hnbg#;@Abv*)Y!2u5V>jnI2o1O>JCqxZgQFUd56@~if07{6*y&z`T^BN&0& z8&hEoiy*7^R#y`2tM-CSGbndZd;V@%xUNg>tIcXc@zrJ{+OAGO|Kc(70=7BTBK1x&LmP_1SUNYZ@eXwE-vnHQh#-;41U-YjzO70s{$ z-_lm$w6th!MaIx52fQKX>z27Z-Hm^a)Hx{OdETlPDeIOQyDkvXM2spmz7?Zm6PI>Q z0n3$aSd1j?ynl?ec7TmN8AhgZK*Janb3M2KgWATu$1vM1OVrhMcRz>o+V%~!ap7LG zfi#vu8BNOx(BgK<2MAIB`IYc_EOzuEf#uuLs%0LX{M^CoQnzMb5vd zp~9PcKT6!Rb0uVDKr_Z-l@BAvnXR6;P(VG#S#ttt->+q>nW~f!T)*j{xGEo(atZ2Y zP$`+B2>J9e{(C#mXf?%Q-hmukqml@%U;s%LX|^Okysq&o9DwmxuEKCa^102USK*ax zhW%9UQgK&O04Uwgek84NQ?F8v;fdQ>rHCggd)Zrdg9Remej1imVw;O3t=3eApiv$p zvZpAR%uNLk`4{)Xk)oSjXNcJ@%v47Y7GS?E!Z#o*0~#55?y~7ypch3$th8q6OlT#mkUj&U11^{WzkVp#sn zaZ1CxSY1yD@$aVeUl}QE+Kw?sT@g>5m-Oz}xkXp}jByjsc_B}d&3Nt` z%LgwQWd_I`N4wb5T0VFQBXnC$_?ayYcDKA7LKnRs02vU)_5+LA##dg~IUXZyHi z3a_H{;LspRdNKieyYwV~3}2;Mp!)676OVhJl%C46itH(}j|UtTFaXCW@5OqX;y|ak zf#6$ZAn9dJ>>;QmTu3ib#OEa*vTy7O2|aNNFp`%Tr1xEiAH+@1F%wDM zMw^dO*-pE@D+O3d4=J`Isp1uCGe*su(H8wEFIZ5L{K01|G7B~URu`-#(Ps1&#Ptf+ z;k-mcDY_rvEA*OmXgc`lmdzFRU3l`#*0t)%FVuDDi+S>E1Jr00+wAbq7fty<$Etvw z>Uh*dHZ;o~JSTuXmYE42!!z_tpHs%si9@sIV{4qXeP+djst zfTs&1U|-*Ph8rW-j6$JN7U3@hj)z&b%Hv6)ENIuEFRCm8U{!q7L|a>2U2t(t=?FS` zVitJh;GZWRy}06e=GQDPDmeg9F-Buk%ypUS_L+eXGD>lz!>PtSlX=U!)?BgKx@J}> z9fN*jch(+qlFC&tb0;Z$n$I~)H$crnPANJk1DuW&CcekVxcXcDL=6iFyu#5i-XX18 z-IV9Yqh*qex{k+6hdac%-yF@E`ZG@cO6RuaX+p272smS}t3dEZ=O6dF#wCx>x`*&` zy}|36*yBk&6no@*1#l8;^ty^JMKm)N(S=@D@4R`YZ-L|W5DX{mnMBWYmLm>lr}0Ld zx@UoZ2zY4!3-@eCuGD+ovndL|?K8p&7`H|rX|}x9sP_M`j}+WeP&ergKGFTRxMFvPt3k~dXinQPT5dQS}C)^L=uMgvX$Rvi53d3^kw0m-l z>aIocE&(oR9ZK)x2%_MT0d=;dX|7)hmPSo~ck;kIhoh zQwzpZOMG}KxX5PhrJ#c(6`Ao4x@LlV(kDF;=Y{tRRYGyU7cvn;X^z#I3I)JqGSmTA zcP_S%Q3E*q`Lf~3&7T)se-xcANs6)j!mb&4MF#st~80=zDK>##kOq+rLsKf{o5hs*@7!LR?iHJEPUtT>*^__8O;vyZP zz~LT6o#+@-7HfHo{Il_&%(YiHRAB&Azo**NR@t3J*;7hiQlt-1>!d-E*6MeB(jQ)? z`uPagn?|ZpJqho+VX0PZG|qX2Cpp%0dQiZ-mf@?Sj1F+Hl~L~EU{kjpUu+##_>KqsKObUQ#|0-KsU^|l1e8njs)_0LrR32#`mUoB@B%OIsVV|p zi%#xW`+L3=eb=BWei2ZOu`M(7 zVi#`1e9T)&rwhPt$lU+{1bs8hzK{szr+NyWe5Rr(CVpvLUHMizGiqYu9(qPrM11OJT8ej57;m>{?m2Opmc#AUmsgp}>erPRPZ>MvO7ypKTSOe*_Y@ zv*6HwLEo4*gAu%%rR#2gI8)p}$wwwkYPn_0;iNp-n<_PMT7sU)YZEab;#N+{X zUb+xRcF$94>)jT&r<6yjPmuTVv|6O>_*vUbhmp)oS~Kl)Eu=uwNGVprxps1eaa2N4 zijr_8{z#I_Z+W^9*9oEXEErVf5>Zmtg>Sy$(8@m8vC> z8XEX_!k_H|xPvz14J0P)yFS35*?8j^-B1cMbCV0ojn-zzaRNGxGR6^z&pgcS#~PyI zLoqlX7%f?eCv^QFM@clk##%B?nOaQ`t{~8-D*}ZeKzDgJg^C(4S0|b5c==ZnrNr=Z zX!Cm*?k{2RmEfmKF!^A`azg2Lr5P^(M+oi-R>pEmw=ji;wPNo&PT_crN}(vub3@mU z*tKyFz>9&qC- z2k5m8#7MK!5TrpUIBL4;asiBRgk2aJN|{Y8cU0za8Xk~F0SI_C7(K%^DMLn#O7^H5 z%SUEA;T*oXuth$O9V1jykH?r@At(D~@G%(El<>2%`Vz-FvP~Fu#Mt;)oWgZCKdx4X zley*<+RvvJKqWDH_I!?yfaoIbld@++*ZXh7{RdO2(p_|H_&XobF*Ij@Ao`U0e4(4< z!}&9_TH-Jh}JnHyv_@nV)KgC;lk>~H1beNyZD*JSc2stizm^upe?*@a9m&zb?> z)s;fG>hQV3Lpw%^kIT-0?h$FG?w uzT!#D=}{O5)FckkHL$UO(!{{_nwx*S20JQ>Z+7$m0000 + + + + + + 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:1901371251.2 %
Date:2023-12-06 07:59:45Functions:7312359.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
50.6%50.6%
+ 		50.6 %1858 / 366955.0 %61 / 111
<unnamed>50.6 %1858 / 366955.0 %61 / 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..c98ca20639 --- /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:1901371251.2 %
Date:2023-12-06 07:59:45Functions:7312359.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
50.6%50.6%
+ 		50.6 %1858 / 366955.0 %61 / 111
<unnamed>50.6 %1858 / 366955.0 %61 / 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..9dd42c14b5 --- /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:1901371251.2 %
Date:2023-12-06 07:59:45Functions:7312359.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
50.6%50.6%
+ 		50.6 %1858 / 366955.0 %61 / 111
<unnamed>50.6 %1858 / 366955.0 %61 / 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..d677b3ed2d --- /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:1901371251.2 %
Date:2023-12-06 07:59:45Functions:7312359.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
50.6%50.6%
+ 		50.6 %1858 / 366955.0 %61 / 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..7ece851662 --- /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:1901371251.2 %
Date:2023-12-06 07:59:45Functions:7312359.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
50.6%50.6%
+ 		50.6 %1858 / 366955.0 %61 / 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..41365d1996 --- /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:1901371251.2 %
Date:2023-12-06 07:59:45Functions:7312359.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
50.6%50.6%
+ 		50.6 %1858 / 366955.0 %61 / 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..391b2a2264 --- /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:2023-12-06 07:59:45Functions: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&)42
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()42
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)349
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)2509
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24482
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&)35422
+
+
+ + + +
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..3e08642939 --- /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:2023-12-06 07:59:45Functions: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&)2509
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)3191
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)348
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)349
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)24482
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)2772
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)342
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)692
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&)35422
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)42
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()42
+
+
+ + + +
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..d42270ad4f --- /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:2023-12-06 07:59:45Functions: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          42 : OutputPublisher::OutputPublisher() {
+      10          42 : }
+      11             : 
+      12          42 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14          42 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15          42 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16          42 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17          42 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18          42 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19          42 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20          42 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21          42 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22          42 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23          42 : }
+      24             : 
+      25       35422 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27       35422 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28       35422 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32        2509 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33        2509 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34        2509 : }
+      35             : 
+      36        2772 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37        2772 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38        2772 : }
+      39             : 
+      40       24482 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41       24482 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42       24482 : }
+      43             : 
+      44        3191 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        3191 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        3191 : }
+      47             : 
+      48         692 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49         692 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50         692 : }
+      51             : 
+      52         737 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53         737 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54         737 : }
+      55             : 
+      56         342 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57         342 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58         342 : }
+      59             : 
+      60         349 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61         349 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62         349 : }
+      63             : 
+      64         348 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65         348 :   ph_hw_api_position_cmd_.publish(msg);
+      66         348 : }
+      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..423769ec08 --- /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:33052862.5 %
Date:2023-12-06 07:59:45Functions:182475.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()0
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::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)42
mrs_uav_managers::estimation_manager::EstimationManager::onInit()42
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()42
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)51
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)104
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const104
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const208
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const1198
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const5089
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)5546
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)53777
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)53787
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const53787
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const59319
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const113092
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const513151
+
+
+ + + +
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..5eda3b00bd --- /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:33052862.5 %
Date:2023-12-06 07:59:45Functions:182475.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)104
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()0
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)42
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)53777
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&)53787
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)42
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)5546
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> >&)51
mrs_uav_managers::estimation_manager::EstimationManager::onInit()42
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()42
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const53787
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const104
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const113092
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const208
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const59319
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const5089
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const513151
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const1198
+
+
+ + + +
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..8ef4b8ff49 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html @@ -0,0 +1,1313 @@ + + + + + + + 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:33052862.5 %
Date:2023-12-06 07:59:45Functions:182475.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*//{ includes */
+       2             : #include <ros/ros.h>
+       3             : #include <nodelet/nodelet.h>
+       4             : 
+       5             : #include <pluginlib/class_loader.h>
+       6             : #include <nodelet/loader.h>
+       7             : 
+       8             : #include <geometry_msgs/QuaternionStamped.h>
+       9             : #include <geometry_msgs/Vector3Stamped.h>
+      10             : 
+      11             : #include <nav_msgs/Odometry.h>
+      12             : 
+      13             : #include <std_srvs/Trigger.h>
+      14             : #include <std_srvs/SetBool.h>
+      15             : 
+      16             : #include <mrs_msgs/UavState.h>
+      17             : #include <mrs_msgs/Float64Stamped.h>
+      18             : #include <mrs_msgs/String.h>
+      19             : #include <mrs_msgs/EstimationDiagnostics.h>
+      20             : #include <mrs_msgs/HwApiCapabilities.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : 
+      23             : #include <mrs_lib/param_loader.h>
+      24             : #include <mrs_lib/publisher_handler.h>
+      25             : #include <mrs_lib/service_client_handler.h>
+      26             : #include <mrs_lib/transformer.h>
+      27             : #include <mrs_lib/subscribe_handler.h>
+      28             : #include <mrs_lib/gps_conversions.h>
+      29             : #include <mrs_lib/scope_timer.h>
+      30             : 
+      31             : 
+      32             : #include <mrs_uav_managers/state_estimator.h>
+      33             : #include <mrs_uav_managers/agl_estimator.h>
+      34             : #include <mrs_uav_managers/estimation_manager/support.h>
+      35             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      36             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      37             : /*//}*/
+      38             : 
+      39             : namespace mrs_uav_managers
+      40             : {
+      41             : 
+      42             : namespace estimation_manager
+      43             : {
+      44             : 
+      45             : // --------------------------------------------------------------
+      46             : // |                           classes                          |
+      47             : // --------------------------------------------------------------
+      48             : 
+      49             : /*//{ class StateMachine */
+      50             : class StateMachine {
+      51             : 
+      52             :   /*//{ states */
+      53             : public:
+      54             :   typedef enum
+      55             :   {
+      56             : 
+      57             :     UNINITIALIZED_STATE,
+      58             :     INITIALIZED_STATE,
+      59             :     READY_FOR_TAKEOFF_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         588 :   StateMachine(const std::string& nodelet_name) : nodelet_name_(nodelet_name) {
+      77          42 :   }
+      78             : 
+      79      513151 :   bool isInState(const SMState_t& state) const {
+      80      513151 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) == state;
+      81             :   }
+      82             : 
+      83      113092 :   bool isInitialized() const {
+      84      113092 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) != UNINITIALIZED_STATE;
+      85             :   }
+      86             : 
+      87       59319 :   bool isInPublishableState() const {
+      88       59319 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      89       18196 :     return current_state == READY_FOR_TAKEOFF_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      90       77519 :            current_state == LANDING_STATE || current_state == DUMMY_STATE || current_state == FAILSAFE_STATE;
+      91             :   }
+      92             : 
+      93       53787 :   bool isInTheAir() const {
+      94       53787 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      95       53787 :     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        5089 :   std::string getCurrentStateString() const {
+     103        5089 :     return mrs_lib::get_mutexed(mtx_state_, sm_state_names_[current_state_]);
+     104             :   }
+     105             : 
+     106         208 :   std::string getStateAsString(const SMState_t& state) const {
+     107         208 :     return sm_state_names_[state];
+     108             :   }
+     109             : 
+     110             :   /*//{ changeState() */
+     111         104 :   bool changeState(const SMState_t& target_state) {
+     112             : 
+     113         104 :     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         104 :     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          42 :       case INITIALIZED_STATE: {
+     129          42 :         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          42 :         break;
+     135             :       }
+     136             : 
+     137          42 :       case READY_FOR_TAKEOFF_STATE: {
+     138          42 :         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_TAKEOFF_STATE).c_str(), getStateAsString(INITIALIZED_STATE).c_str(),
+     141             :                              getStateAsString(LANDED_STATE).c_str());
+     142           0 :           return false;
+     143             :         }
+     144          42 :         break;
+     145             :       }
+     146             : 
+     147          10 :       case TAKING_OFF_STATE: {
+     148          10 :         if (current_state_ != READY_FOR_TAKEOFF_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_TAKEOFF_STATE).c_str());
+     151           0 :           return false;
+     152             :         }
+     153          10 :         break;
+     154             :       }
+     155             : 
+     156          10 :       case FLYING_STATE: {
+     157          10 :         if (current_state_ != TAKING_OFF_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          10 :         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           0 :       case ESTIMATOR_SWITCHING_STATE: {
+     176           0 :         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           0 :         pre_switch_state_ = current_state_;
+     183           0 :         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         104 :     std::scoped_lock lock(mtx_state_);
+     235             :     {
+     236         104 :       previous_state_ = current_state_;
+     237         104 :       current_state_  = target_state;
+     238             :     }
+     239             : 
+     240         104 :     ROS_INFO("[%s]: successfully changed states %s -> %s", getPrintName().c_str(), getStateAsString(previous_state_).c_str(),
+     241             :              getStateAsString(current_state_).c_str());
+     242             : 
+     243         104 :     return true;
+     244             :   }
+     245             :   /*//}*/
+     246             : 
+     247             :   /*//{ changeToPreSwitchState() */
+     248           0 :   void changeToPreSwitchState() {
+     249           0 :     changeState(pre_switch_state_);
+     250           0 :   }
+     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         104 :   std::string getPrintName() const {
+     268         208 :     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_TAKEOFF_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             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+     309             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>            sh_control_input_;
+     310             : 
+     311             :   mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics> ph_diagnostics_;
+     312             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>        ph_max_flight_z_;
+     313             :   mrs_lib::PublisherHandler<mrs_msgs::UavState>              ph_uav_state_;
+     314             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_odom_main_;
+     315             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_odom_slow_;  // use topic throttler instead?
+     316             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_innovation_;
+     317             : 
+     318             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_altitude_amsl_;
+     319             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_altitude_agl_;
+     320             : 
+     321             :   mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped> ph_orientation_;
+     322             : 
+     323             :   ros::Timer timer_publish_;
+     324             :   void       timerPublish(const ros::TimerEvent& event);
+     325             : 
+     326             :   ros::Timer timer_publish_diagnostics_;
+     327             :   void       timerPublishDiagnostics(const ros::TimerEvent& event);
+     328             : 
+     329             :   ros::Timer timer_check_health_;
+     330             :   void       timerCheckHealth(const ros::TimerEvent& event);
+     331             : 
+     332             :   ros::Timer timer_initialization_;
+     333             :   void       timerInitialization(const ros::TimerEvent& event);
+     334             : 
+     335             :   ros::ServiceServer srvs_change_estimator_;
+     336             :   bool               callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     337             :   int                estimator_switch_count_ = 0;
+     338             : 
+     339             : 
+     340             :   ros::ServiceServer srvs_toggle_callbacks_;
+     341             :   bool               callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     342             :   bool               callbacks_enabled_             = false;
+     343             :   bool               callbacks_disabled_by_service_ = false;
+     344             : 
+     345             :   bool                                             callFailsafeService();
+     346             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvch_failsafe_;
+     347             :   bool                                             failsafe_call_succeeded_ = false;
+     348             : 
+     349             :   // TODO service clients
+     350             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_hover_; */
+     351             :   /* mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> srvc_reference_; */
+     352             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_ehover_; */
+     353             :   /* mrs_lib::ServiceClientHandler<std_srvs::SetBool> srvc_enable_callbacks_; */
+     354             : 
+     355             :   // | ------------- dynamic loading of estimators ------------- |
+     356             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>> state_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     357             :   std::vector<std::string>                                                  estimator_names_;         // list of estimator names
+     358             :   std::vector<boost::shared_ptr<mrs_uav_managers::StateEstimator>>          estimator_list_;          // list of estimators
+     359             :   std::mutex                                                                mutex_estimator_list_;
+     360             :   std::vector<std::string>                                                  switchable_estimator_names_;
+     361             :   std::mutex                                                                mutex_switchable_estimator_names_;
+     362             :   std::string                                                               initial_estimator_name_ = "UNDEFINED_INITIAL_ESTIMATOR";
+     363             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       initial_estimator_;
+     364             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       active_estimator_;
+     365             :   std::mutex                                                                mutex_active_estimator_;
+     366             : 
+     367             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>> agl_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     368             :   std::string                                                             est_alt_agl_name_ = "UNDEFINED_AGL_ESTIMATOR";
+     369             :   boost::shared_ptr<mrs_uav_managers::AglEstimator>                       est_alt_agl_;
+     370             :   bool                                                                    is_using_agl_estimator_;
+     371             : 
+     372             :   double max_flight_z_;
+     373             : 
+     374             :   bool switchToHealthyEstimator();
+     375             :   void switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator);
+     376             : 
+     377             :   bool loadConfigFile(const std::string& file_path);
+     378             : 
+     379             : public:
+     380          42 :   EstimationManager() {
+     381          42 :   }
+     382             : 
+     383             :   void onInit();
+     384             : 
+     385             :   std::string getName() const;
+     386             : };
+     387             : /*//}*/
+     388             : 
+     389             : /*//{ onInit() */
+     390          42 : void EstimationManager::onInit() {
+     391             : 
+     392          42 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     393             : 
+     394          42 :   ros::Time::waitForValid();
+     395             : 
+     396          42 :   ROS_INFO("[%s]: initializing", getName().c_str());
+     397             : 
+     398          42 :   sm_ = std::make_shared<StateMachine>(nodelet_name_);
+     399             : 
+     400          42 :   ch_ = std::make_shared<CommonHandlers_t>();
+     401             : 
+     402          42 :   ch_->nodelet_name = nodelet_name_;
+     403          42 :   ch_->package_name = package_name_;
+     404             : 
+     405             :   // finish initialization in a oneshot timer, so that we don't block loading of other nodelets by the nodelet manager
+     406          42 :   timer_initialization_ = nh_.createTimer(ros::Rate(1.0), &EstimationManager::timerInitialization, this, true, true);
+     407          42 : }
+     408             : /*//}*/
+     409             : 
+     410             : // --------------------------------------------------------------
+     411             : // |                          callbacks                         |
+     412             : // --------------------------------------------------------------
+     413             : 
+     414             : // | --------------------- timer callbacks -------------------- |
+     415             : 
+     416             : /*//{ timerPublish() */
+     417       53777 : void EstimationManager::timerPublish([[maybe_unused]] const ros::TimerEvent& event) {
+     418             : 
+     419       53777 :   if (!sm_->isInitialized()) {
+     420        7355 :     return;
+     421             :   }
+     422             : 
+     423      107554 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublish", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     424             : 
+     425       53777 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     426           0 :     ROS_WARN("[%s]: Not publishing during estimator switching.", getName().c_str());
+     427           0 :     return;
+     428             :   }
+     429             : 
+     430       53777 :   if (!sm_->isInPublishableState()) {
+     431        7355 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     432        7355 :     return;
+     433             :   }
+     434             : 
+     435       46422 :   mrs_msgs::UavState uav_state;
+     436       46422 :   auto               ret = active_estimator_->getUavState();
+     437       46422 :   if (ret) {
+     438       46422 :     uav_state = ret.value();
+     439             :   } else {
+     440           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     441           0 :     return;
+     442             :   }
+     443             : 
+     444       46422 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     445           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     446           0 :     return;
+     447             :   }
+     448             : 
+     449             : 
+     450       46422 :   if (active_estimator_->isMitigatingJump()) {
+     451           0 :     estimator_switch_count_++;
+     452             :   }
+     453       46422 :   uav_state.estimator_iteration = estimator_switch_count_;
+     454             : 
+     455       46422 :   scope_timer.checkpoint("get uav state");
+     456             :   // TODO state health checks
+     457             : 
+     458       46422 :   ph_uav_state_.publish(uav_state);
+     459             : 
+     460       46422 :   scope_timer.checkpoint("pub uav state");
+     461       92844 :   nav_msgs::Odometry odom_main = Support::uavStateToOdom(uav_state);
+     462             : 
+     463       46422 :   scope_timer.checkpoint("uav state to odom");
+     464       92844 :   const std::vector<double> pose_covariance = active_estimator_->getPoseCovariance();
+     465     1717578 :   for (size_t i = 0; i < pose_covariance.size(); i++) {
+     466     1671156 :     odom_main.pose.covariance[i] = pose_covariance[i];
+     467             :   }
+     468             : 
+     469       92844 :   const std::vector<double> twist_covariance = active_estimator_->getTwistCovariance();
+     470     1717578 :   for (size_t i = 0; i < twist_covariance.size(); i++) {
+     471     1671156 :     odom_main.twist.covariance[i] = twist_covariance[i];
+     472             :   }
+     473             : 
+     474       46422 :   scope_timer.checkpoint("get covariance");
+     475       46422 :   ph_odom_main_.publish(odom_main);
+     476             : 
+     477       92844 :   nav_msgs::Odometry innovation = active_estimator_->getInnovation();
+     478       46422 :   ph_innovation_.publish(innovation);
+     479             : 
+     480             : 
+     481       92844 :   mrs_msgs::Float64Stamped agl_height;
+     482             : 
+     483       46422 :   if (is_using_agl_estimator_) {
+     484       38272 :     agl_height = est_alt_agl_->getUavAglHeight();
+     485       38272 :     ph_altitude_agl_.publish(agl_height);
+     486             :   }
+     487             : }
+     488             : /*//}*/
+     489             : 
+     490             : /*//{ timerPublishDiagnostics() */
+     491        5546 : void EstimationManager::timerPublishDiagnostics([[maybe_unused]] const ros::TimerEvent& event) {
+     492             : 
+     493        5546 :   if (!sm_->isInitialized()) {
+     494         737 :     return;
+     495             :   }
+     496             : 
+     497       11092 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublishDiagnostics", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     498             : 
+     499        5546 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     500           0 :     ROS_WARN("[%s]: Not publishing diagnostics during estimator switching.", getName().c_str());
+     501           0 :     return;
+     502             :   }
+     503             : 
+     504        5546 :   if (!sm_->isInPublishableState()) {
+     505         737 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     506         737 :     return;
+     507             :   }
+     508             : 
+     509        4809 :   mrs_msgs::UavState uav_state;
+     510        4809 :   auto               ret = active_estimator_->getUavState();
+     511        4809 :   if (ret) {
+     512        4809 :     uav_state = ret.value();
+     513             :   } else {
+     514           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     515           0 :     return;
+     516             :   }
+     517             : 
+     518        4809 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     519           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     520           0 :     return;
+     521             :   }
+     522             : 
+     523        9618 :   mrs_msgs::Float64Stamped agl_height;
+     524             : 
+     525        4809 :   if (is_using_agl_estimator_) {
+     526        3993 :     agl_height = est_alt_agl_->getUavAglHeight();
+     527        3993 :     ph_altitude_agl_.publish(agl_height);
+     528             :   }
+     529             : 
+     530        9618 :   mrs_msgs::Float64Stamped max_flight_z_msg;
+     531        4809 :   max_flight_z_msg.header.stamp = ros::Time::now();
+     532        4809 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     533        4809 :     max_flight_z_msg.header.frame_id = active_estimator_->getFrameId();
+     534        4809 :     max_flight_z_msg.value           = active_estimator_->getMaxFlightZ();
+     535             :   }
+     536        4809 :   max_flight_z_ = max_flight_z_msg.value;
+     537        4809 :   ph_max_flight_z_.publish(max_flight_z_msg);
+     538             : 
+     539             :   // publish diagnostics
+     540        9618 :   mrs_msgs::EstimationDiagnostics diagnostics;
+     541             : 
+     542        4809 :   diagnostics.header.stamp   = uav_state.header.stamp;
+     543        4809 :   diagnostics.child_frame_id = uav_state.child_frame_id;
+     544             : 
+     545        4809 :   diagnostics.pose         = uav_state.pose;
+     546        4809 :   diagnostics.velocity     = uav_state.velocity;
+     547        4809 :   diagnostics.acceleration = uav_state.acceleration;
+     548             : 
+     549        4809 :   diagnostics.sm_state                 = sm_->getCurrentStateString();
+     550        4809 :   diagnostics.max_flight_z             = max_flight_z_msg.value;
+     551        4809 :   diagnostics.estimator_iteration      = estimator_switch_count_;
+     552        4809 :   diagnostics.estimation_rate          = ch_->desired_uav_state_rate;
+     553        4809 :   diagnostics.running_state_estimators = estimator_names_;
+     554             : 
+     555             :   {
+     556        9618 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     557        4809 :     diagnostics.switchable_state_estimators = switchable_estimator_names_;
+     558             :   }
+     559             : 
+     560             : 
+     561        4809 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     562        4809 :     diagnostics.header.frame_id         = active_estimator_->getFrameId();
+     563        4809 :     diagnostics.current_state_estimator = active_estimator_->getName();
+     564             :   } else {
+     565           0 :     diagnostics.header.frame_id         = "";
+     566           0 :     diagnostics.current_state_estimator = "";
+     567             :   }
+     568             : 
+     569        4809 :   diagnostics.estimator_horizontal = uav_state.estimator_horizontal;
+     570        4809 :   diagnostics.estimator_vertical   = uav_state.estimator_vertical;
+     571        4809 :   diagnostics.estimator_heading    = uav_state.estimator_heading;
+     572             : 
+     573        4809 :   if (is_using_agl_estimator_) {
+     574        3993 :     diagnostics.estimator_agl_height = est_alt_agl_name_;
+     575        3993 :     diagnostics.agl_height           = agl_height.value;
+     576             :   } else {
+     577         816 :     diagnostics.estimator_agl_height = "NOT_USED";
+     578         816 :     diagnostics.agl_height           = std::nanf("");
+     579             :   }
+     580             : 
+     581        4809 :   diagnostics.platform_config = _platform_config_;
+     582        4809 :   diagnostics.custom_config   = _custom_config_;
+     583             : 
+     584        4809 :   ph_diagnostics_.publish(diagnostics);
+     585             : 
+     586        4809 :   ROS_INFO_THROTTLE(5.0, "[%s]: %s. pos: [%.2f, %.2f, %.2f] m. Estimator: %s. Max. z.: %.2f m. Estimator switches: %d.", getName().c_str(),
+     587             :                     sm_->getCurrentStateString().c_str(), uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z,
+     588             :                     active_estimator_->getName().c_str(), max_flight_z_, estimator_switch_count_);
+     589             : }
+     590             : /*//}*/
+     591             : 
+     592             : /*//{ timerCheckHealth() */
+     593       53787 : void EstimationManager::timerCheckHealth([[maybe_unused]] const ros::TimerEvent& event) {
+     594             : 
+     595       53787 :   if (!sm_->isInitialized()) {
+     596           0 :     return;
+     597             :   }
+     598             : 
+     599      161361 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     600             : 
+     601             :   /*//{ start ready estimators, check switchable estimators */
+     602      107574 :   std::vector<std::string> switchable_estimator_names;
+     603      107574 :   for (auto estimator : estimator_list_) {
+     604             : 
+     605       53787 :     if (estimator->isReady()) {
+     606             :       try {
+     607         696 :         ROS_INFO_THROTTLE(1.0, "[%s]: starting the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     608         696 :         estimator->start();
+     609             :       }
+     610           0 :       catch (std::runtime_error& ex) {
+     611           0 :         ROS_ERROR("[%s]: exception caught during estimator starting: '%s'", getName().c_str(), ex.what());
+     612           0 :         ros::shutdown();
+     613             :       }
+     614             :     }
+     615             : 
+     616       53787 :     if (estimator->isRunning() && estimator->getName() != "dummy" && estimator->getName() != "ground_truth") {
+     617       52145 :       switchable_estimator_names.push_back(estimator->getName());
+     618             :     }
+     619             :   }
+     620             : 
+     621             :   {
+     622      107574 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     623       53787 :     switchable_estimator_names_ = switchable_estimator_names;
+     624             :   }
+     625             : 
+     626       53787 :   if (is_using_agl_estimator_ && est_alt_agl_->isReady()) {
+     627          35 :     est_alt_agl_->start();
+     628             :   }
+     629             : 
+     630             :   /*//}*/
+     631             : 
+     632       98775 :   if (!callbacks_disabled_by_service_ &&
+     633       98775 :       (sm_->isInState(StateMachine::FLYING_STATE) || sm_->isInState(StateMachine::HOVER_STATE) || sm_->isInState(StateMachine::READY_FOR_TAKEOFF_STATE))) {
+     634       37594 :     callbacks_enabled_ = true;
+     635             :   } else {
+     636       16193 :     callbacks_enabled_ = false;
+     637             :   }
+     638             : 
+     639             :   // TODO fuj if, zmenit na switch
+     640             :   // activate initial estimator
+     641       53787 :   if (sm_->isInState(StateMachine::INITIALIZED_STATE) && initial_estimator_->isRunning()) {
+     642       11446 :     std::scoped_lock lock(mutex_active_estimator_);
+     643        5723 :     ROS_INFO_THROTTLE(1.0, "[%s]: activating the initial estimator %s", getName().c_str(), initial_estimator_->getName().c_str());
+     644        5723 :     active_estimator_ = initial_estimator_;
+     645        5723 :     if (active_estimator_->getName() == "dummy") {
+     646           0 :       sm_->changeState(StateMachine::DUMMY_STATE);
+     647             :     } else {
+     648        5723 :       if (!is_using_agl_estimator_ || est_alt_agl_->isRunning()) {
+     649          42 :         sm_->changeState(StateMachine::READY_FOR_TAKEOFF_STATE);
+     650             :       } else {
+     651        5681 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s agl estimator: %s to be running", getName().c_str(), Support::waiting_for_string.c_str(),
+     652             :                           est_alt_agl_->getName().c_str());
+     653             :       }
+     654             :     }
+     655             :   }
+     656             : 
+     657             :   // active estimator is in faulty state, we need to switch to healthy estimator
+     658       53787 :   if (sm_->isInTheAir() && active_estimator_->isError()) {
+     659           0 :     sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+     660             :   }
+     661             : 
+     662       53787 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     663           0 :     if (switchToHealthyEstimator()) {
+     664           0 :       sm_->changeToPreSwitchState();
+     665             :     } else {  // cannot switch to healthy estimator - failsafe necessary
+     666           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Cannot switch to any healthy estimator. Triggering failsafe.", getName().c_str());
+     667           0 :       sm_->changeState(StateMachine::FAILSAFE_STATE);
+     668             :     }
+     669             :   }
+     670             : 
+     671       53787 :   if (sm_->isInState(StateMachine::FAILSAFE_STATE)) {
+     672           0 :     if (!failsafe_call_succeeded_ && callFailsafeService()) {
+     673           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: failsafe called successfully", getName().c_str());
+     674           0 :       failsafe_call_succeeded_ = true;
+     675             :     }
+     676           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: we are in failsafe state", getName().c_str());
+     677             :   }
+     678             : 
+     679       53787 :   if (sm_->isInState(StateMachine::READY_FOR_TAKEOFF_STATE)) {
+     680       37283 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == "LandoffTracker") {
+     681          10 :       sm_->changeState(StateMachine::TAKING_OFF_STATE);
+     682             :     }
+     683             :   }
+     684             : 
+     685       53787 :   if (sm_->isInState(StateMachine::TAKING_OFF_STATE)) {
+     686        6230 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker != "LandoffTracker") {
+     687          10 :       sm_->changeState(StateMachine::FLYING_STATE);
+     688             :     }
+     689             :   }
+     690             : 
+     691       53787 :   if (sm_->isInState(StateMachine::FLYING_STATE)) {
+     692        2971 :     if ((ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     693         152 :       ROS_WARN_THROTTLE(1.0, "[%s]: not received control input for %.4fs, estimation suboptimal, potentially unstable", getName().c_str(),
+     694             :                         (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     695             :     }
+     696             :   }
+     697             : }
+     698             : /*//}*/
+     699             : 
+     700             : /*//{ timerInitialization() */
+     701          42 : void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEvent& event) {
+     702             : 
+     703          84 :   mrs_lib::ParamLoader param_loader(nh_, getName());
+     704             : 
+     705          42 :   param_loader.loadParam("custom_config", _custom_config_);
+     706          42 :   param_loader.loadParam("platform_config", _platform_config_);
+     707          42 :   param_loader.loadParam("world_config", _world_config_);
+     708             : 
+     709          42 :   if (_custom_config_ != "") {
+     710          42 :     param_loader.addYamlFile(_custom_config_);
+     711             :   }
+     712             : 
+     713          42 :   if (_platform_config_ != "") {
+     714          42 :     param_loader.addYamlFile(_platform_config_);
+     715             :   }
+     716             : 
+     717          42 :   if (_world_config_ != "") {
+     718          42 :     param_loader.addYamlFile(_world_config_);
+     719             :   }
+     720             : 
+     721          42 :   param_loader.addYamlFileFromParam("private_config");
+     722          42 :   param_loader.addYamlFileFromParam("public_config");
+     723          42 :   param_loader.addYamlFileFromParam("estimators_config");
+     724          42 :   param_loader.addYamlFileFromParam("active_estimators_config");
+     725             : 
+     726          42 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     727             : 
+     728             :   /*//{ load world_origin parameters */
+     729             : 
+     730          84 :   std::string world_origin_units;
+     731          42 :   bool        is_origin_param_ok = true;
+     732          42 :   double      world_origin_x     = 0;
+     733          42 :   double      world_origin_y     = 0;
+     734             : 
+     735          42 :   param_loader.loadParam("world_origin/units", world_origin_units);
+     736             : 
+     737          42 :   if (Support::toLowercase(world_origin_units) == "utm") {
+     738           0 :     ROS_INFO("[%s]: Loading world origin in UTM units.", getName().c_str());
+     739           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", world_origin_x);
+     740           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", world_origin_y);
+     741             : 
+     742          42 :   } else if (Support::toLowercase(world_origin_units) == "latlon") {
+     743             :     double lat, lon;
+     744          42 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getName().c_str());
+     745          42 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     746          42 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     747          42 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     748          42 :     ROS_INFO("[%s]: Converted to UTM x: %f, y: %f.", getName().c_str(), world_origin_x, world_origin_y);
+     749             : 
+     750             :   } else {
+     751           0 :     ROS_ERROR("[%s]: world_origin_units must be (\"UTM\"|\"LATLON\"). Got '%s'", getName().c_str(), world_origin_units.c_str());
+     752           0 :     ros::shutdown();
+     753             :   }
+     754             : 
+     755          42 :   ch_->world_origin.x = world_origin_x;
+     756          42 :   ch_->world_origin.y = world_origin_y;
+     757             : 
+     758          42 :   if (!is_origin_param_ok) {
+     759           0 :     ROS_ERROR("[%s]: Could not load all mandatory parameters from world file. Please check your world file.", getName().c_str());
+     760           0 :     ros::shutdown();
+     761             :   }
+     762             :   /*//}*/
+     763             : 
+     764          42 :   param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/");
+     765             : 
+     766             :   /*//{ load parameters into common handlers */
+     767          42 :   param_loader.loadParam("frame_id/fcu", ch_->frames.fcu);
+     768          42 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + ch_->frames.fcu;
+     769             : 
+     770          42 :   param_loader.loadParam("frame_id/fcu_untilted", ch_->frames.fcu_untilted);
+     771          42 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + ch_->frames.fcu_untilted;
+     772             : 
+     773          42 :   param_loader.loadParam("frame_id/rtk_antenna", ch_->frames.rtk_antenna);
+     774          42 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     775             : 
+     776          42 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getName());
+     777          42 :   ch_->transformer->retryLookupNewest(true);
+     778             : 
+     779          42 :   param_loader.loadParam("rate/diagnostics", ch_->desired_diagnostics_rate);
+     780             : 
+     781             :   /*//}*/
+     782             : 
+     783             :   /*//{ load parameters */
+     784             : 
+     785             :   /*//{ publish debug topics parameters */
+     786          42 :   param_loader.loadParam("debug_topics/input", ch_->debug_topics.input);
+     787          42 :   param_loader.loadParam("debug_topics/output", ch_->debug_topics.output);
+     788          42 :   param_loader.loadParam("debug_topics/state", ch_->debug_topics.state);
+     789          42 :   param_loader.loadParam("debug_topics/covariance", ch_->debug_topics.covariance);
+     790          42 :   param_loader.loadParam("debug_topics/innovation", ch_->debug_topics.innovation);
+     791          42 :   param_loader.loadParam("debug_topics/diagnostics", ch_->debug_topics.diag);
+     792          42 :   param_loader.loadParam("debug_topics/correction", ch_->debug_topics.correction);
+     793          42 :   param_loader.loadParam("debug_topics/correction_delay", ch_->debug_topics.corr_delay);
+     794             :   /*//}*/
+     795             : 
+     796             :   /*//}*/
+     797             : 
+     798          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+     799          42 :   shopts.nh                 = nh_;
+     800          42 :   shopts.node_name          = getName();
+     801          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     802          42 :   shopts.threadsafe         = true;
+     803          42 :   shopts.autostart          = true;
+     804          42 :   shopts.queue_size         = 10;
+     805          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     806             : 
+     807             :   /*//{ wait for hw api message */
+     808             : 
+     809             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_ =
+     810         126 :       mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     811          85 :   while (!sh_hw_api_capabilities_.hasMsg()) {
+     812          43 :     ROS_INFO("[%s]: %s hw_api_capabilities message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     813             :              sh_hw_api_capabilities_.topicName().c_str());
+     814          43 :     ros::Duration(1.0).sleep();
+     815             :   }
+     816             : 
+     817          84 :   mrs_msgs::HwApiCapabilitiesConstPtr hw_api_capabilities = sh_hw_api_capabilities_.getMsg();
+     818             :   /*//}*/
+     819             : 
+     820             :   /*//{ wait for desired uav_state rate */
+     821          42 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     822          91 :   while (!sh_control_manager_diag_.hasMsg()) {
+     823          49 :     ROS_INFO("[%s]: %s control_manager_diagnostics message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     824             :              sh_control_manager_diag_.topicName().c_str());
+     825          49 :     ros::Duration(1.0).sleep();
+     826             :   }
+     827             : 
+     828          84 :   mrs_msgs::ControlManagerDiagnosticsConstPtr control_manager_diag_msg = sh_control_manager_diag_.getMsg();
+     829          42 :   ch_->desired_uav_state_rate                                          = control_manager_diag_msg->desired_uav_state_rate;
+     830          42 :   ROS_INFO("[%s]: The estimation is running at: %.2f Hz", getName().c_str(), ch_->desired_uav_state_rate);
+     831             :   /*//}*/
+     832             : 
+     833             :   /*//{ initialize subscribers */
+     834             : 
+     835          42 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     836             : 
+     837             :   /*//}*/
+     838             : 
+     839             :   /*//{ load state estimator plugins */
+     840          42 :   param_loader.loadParam("state_estimators", estimator_names_);
+     841             : 
+     842          42 :   state_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>>("mrs_uav_managers", "mrs_uav_managers::StateEstimator");
+     843             : 
+     844          84 :   for (size_t i = 0; i < estimator_names_.size(); i++) {
+     845             : 
+     846          84 :     const std::string estimator_name = estimator_names_[i];
+     847             : 
+     848             :     // load the estimator parameters
+     849          84 :     std::string address;
+     850          42 :     param_loader.loadParam(estimator_name + "/address", address);
+     851             : 
+     852             :     try {
+     853          42 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     854          42 :       estimator_list_.push_back(state_estimator_loader_->createInstance(address.c_str()));
+     855             :     }
+     856           0 :     catch (pluginlib::CreateClassException& ex1) {
+     857           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     858           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     859           0 :       ros::shutdown();
+     860             :     }
+     861           0 :     catch (pluginlib::PluginlibException& ex) {
+     862           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     863           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     864           0 :       ros::shutdown();
+     865             :     }
+     866             :   }
+     867             : 
+     868             :   /*//{ load agl estimator plugins */
+     869          42 :   param_loader.loadParam("agl_height_estimator", est_alt_agl_name_);
+     870          42 :   is_using_agl_estimator_ = est_alt_agl_name_ != "";
+     871          42 :   ROS_WARN_COND(!is_using_agl_estimator_, "[%s]: not using AGL estimator for min height safe checking", getName().c_str());
+     872             : 
+     873             : 
+     874          42 :   if (is_using_agl_estimator_) {
+     875             : 
+     876          35 :     agl_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>>("mrs_uav_managers", "mrs_uav_managers::AglEstimator");
+     877             : 
+     878             :     // load the estimator parameters
+     879          70 :     std::string address;
+     880          35 :     param_loader.loadParam(est_alt_agl_name_ + "/address", address);
+     881             : 
+     882             :     try {
+     883          35 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     884          35 :       est_alt_agl_ = agl_estimator_loader_->createInstance(address.c_str());
+     885             :     }
+     886           0 :     catch (pluginlib::CreateClassException& ex1) {
+     887           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     888           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     889           0 :       ros::shutdown();
+     890             :     }
+     891           0 :     catch (pluginlib::PluginlibException& ex) {
+     892           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     893           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     894           0 :       ros::shutdown();
+     895             :     }
+     896             :   }
+     897             :   /*//}*/
+     898             : 
+     899          42 :   ROS_INFO("[%s]: estimators were loaded", getName().c_str());
+     900             :   /*//}*/
+     901             : 
+     902             :   /*//{ check whether initial estimator was loaded */
+     903          42 :   param_loader.loadParam("initial_state_estimator", initial_estimator_name_);
+     904          42 :   bool initial_estimator_found = false;
+     905          42 :   for (auto estimator : estimator_list_) {
+     906          42 :     if (estimator->getName() == initial_estimator_name_) {
+     907          42 :       initial_estimator_      = estimator;
+     908          42 :       initial_estimator_found = true;
+     909          42 :       break;
+     910             :     }
+     911             :   }
+     912             : 
+     913          42 :   if (!initial_estimator_found) {
+     914           0 :     ROS_ERROR("[%s]: initial estimator %s could not be found among loaded estimators. shutting down", getName().c_str(), initial_estimator_name_.c_str());
+     915           0 :     ros::shutdown();
+     916             :   }
+     917             :   /*//}*/
+     918             : 
+     919             :   /*//{ initialize estimators */
+     920          84 :   for (auto estimator : estimator_list_) {
+     921             : 
+     922             :     // create private handlers
+     923          84 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     924             : 
+     925          42 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     926          42 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
+     927             : 
+     928          42 :     if (_custom_config_ != "") {
+     929          42 :       ph->param_loader->addYamlFile(_custom_config_);
+     930             :     }
+     931             : 
+     932          42 :     if (_platform_config_ != "") {
+     933          42 :       ph->param_loader->addYamlFile(_platform_config_);
+     934             :     }
+     935             : 
+     936          42 :     if (_world_config_ != "") {
+     937          42 :       ph->param_loader->addYamlFile(_world_config_);
+     938             :     }
+     939             : 
+     940             :     try {
+     941          42 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     942          42 :       estimator->initialize(nh_, ch_, ph);
+     943             :     }
+     944           0 :     catch (std::runtime_error& ex) {
+     945           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+     946           0 :       ros::shutdown();
+     947             :     }
+     948             : 
+     949          42 :     if (!estimator->isCompatibleWithHwApi(hw_api_capabilities)) {
+     950           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), estimator->getName().c_str());
+     951           0 :       ros::shutdown();
+     952             :     }
+     953             :   }
+     954             : 
+     955             :   // | ----------- agl height estimator initialization ---------- |
+     956          42 :   if (is_using_agl_estimator_) {
+     957             : 
+     958          70 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     959             : 
+     960          35 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     961          35 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
+     962             : 
+     963          35 :     if (_custom_config_ != "") {
+     964          35 :       ph->param_loader->addYamlFile(_custom_config_);
+     965             :     }
+     966             : 
+     967          35 :     if (_platform_config_ != "") {
+     968          35 :       ph->param_loader->addYamlFile(_platform_config_);
+     969             :     }
+     970             : 
+     971          35 :     if (_world_config_ != "") {
+     972          35 :       ph->param_loader->addYamlFile(_world_config_);
+     973             :     }
+     974             : 
+     975             :     try {
+     976          35 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), est_alt_agl_->getName().c_str());
+     977          35 :       est_alt_agl_->initialize(nh_, ch_, ph);
+     978             :     }
+     979           0 :     catch (std::runtime_error& ex) {
+     980           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+     981           0 :       ros::shutdown();
+     982             :     }
+     983             : 
+     984          35 :     if (!est_alt_agl_->isCompatibleWithHwApi(hw_api_capabilities)) {
+     985           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), est_alt_agl_->getName().c_str());
+     986           0 :       ros::shutdown();
+     987             :     }
+     988             :   }
+     989             : 
+     990          42 :   ROS_INFO("[%s]: estimators were initialized", getName().c_str());
+     991             : 
+     992             :   /*//}*/
+     993             : 
+     994             :   /*//{ initialize publishers */
+     995          42 :   ph_uav_state_    = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh_, "uav_state_out", 10);
+     996          42 :   ph_odom_main_    = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "odom_main_out", 10);
+     997          42 :   ph_innovation_   = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "innovation_out", 10);
+     998          42 :   ph_diagnostics_  = mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics>(nh_, "diagnostics_out", 10);
+     999          42 :   ph_max_flight_z_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "max_flight_z_agl_out", 10);
+    1000          42 :   ph_altitude_agl_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "height_agl_out", 10);
+    1001             : 
+    1002             :   /*//}*/
+    1003             : 
+    1004             :   /*//{ initialize timers */
+    1005          42 :   timer_publish_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerPublish, this);
+    1006             : 
+    1007          42 :   timer_publish_diagnostics_ = nh_.createTimer(ros::Rate(ch_->desired_diagnostics_rate), &EstimationManager::timerPublishDiagnostics, this);
+    1008             : 
+    1009          42 :   timer_check_health_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerCheckHealth, this);
+    1010             :   /*//}*/
+    1011             : 
+    1012             :   /*//{ initialize service clients */
+    1013             : 
+    1014          42 :   srvch_failsafe_.initialize(nh_, "failsafe_out");
+    1015             : 
+    1016             :   /*//}*/
+    1017             : 
+    1018             :   /*//{ initialize service servers */
+    1019          42 :   srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this);
+    1020          42 :   srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this);
+    1021             :   /*//}*/
+    1022             : 
+    1023             :   /*//{ initialize scope timer */
+    1024          42 :   param_loader.loadParam("scope_timer/enabled", ch_->scope_timer.enabled);
+    1025          84 :   std::string       filepath;
+    1026          84 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/scope_timer.txt";
+    1027          42 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+    1028             :   /*//}*/
+    1029             : 
+    1030          42 :   if (!param_loader.loadedSuccessfully()) {
+    1031           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getName().c_str());
+    1032           0 :     ros::shutdown();
+    1033             :   }
+    1034             : 
+    1035          42 :   sm_->changeState(StateMachine::INITIALIZED_STATE);
+    1036             : 
+    1037          42 :   ROS_INFO("[%s]: initialized", getName().c_str());
+    1038          42 : }
+    1039             : /*//}*/
+    1040             : 
+    1041             : // | -------------------- service callbacks ------------------- |
+    1042             : 
+    1043             : /*//{ callbackChangeEstimator() */
+    1044           0 : bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    1045             : 
+    1046           0 :   if (!sm_->isInitialized()) {
+    1047           0 :     return false;
+    1048             :   }
+    1049             : 
+    1050           0 :   if (!callbacks_enabled_) {
+    1051           0 :     res.success = false;
+    1052           0 :     res.message = ("Service callbacks are disabled");
+    1053           0 :     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    1054           0 :     return true;
+    1055             :   }
+    1056             : 
+    1057           0 :   if (req.value == "dummy" || req.value == "ground_truth") {
+    1058           0 :     res.success = false;
+    1059           0 :     std::stringstream ss;
+    1060           0 :     ss << "Switching to " << req.value << " estimator is not allowed.";
+    1061           0 :     res.message = ss.str();
+    1062           0 :     ROS_WARN("[%s]: Switching to %s estimator is not allowed.", getName().c_str(), req.value.c_str());
+    1063           0 :     return true;
+    1064             :   }
+    1065             : 
+    1066             :   // we do not want the switch to be disturbed by a service call
+    1067           0 :   callbacks_enabled_ = false;
+    1068             : 
+    1069           0 :   bool                                                target_estimator_found = false;
+    1070           0 :   boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+    1071           0 :   for (auto estimator : estimator_list_) {
+    1072           0 :     if (estimator->getName() == req.value) {
+    1073           0 :       target_estimator       = estimator;
+    1074           0 :       target_estimator_found = true;
+    1075           0 :       break;
+    1076             :     }
+    1077             :   }
+    1078             : 
+    1079           0 :   if (target_estimator_found) {
+    1080             : 
+    1081           0 :     if (target_estimator->isRunning()) {
+    1082           0 :       sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+    1083           0 :       switchToEstimator(target_estimator);
+    1084           0 :       sm_->changeToPreSwitchState();
+    1085             :     } else {
+    1086           0 :       ROS_WARN("[%s]: Switch to not running estimator %s requested", getName().c_str(), req.value.c_str());
+    1087           0 :       res.success = false;
+    1088           0 :       res.message = ("Requested estimator is not running");
+    1089           0 :       return true;
+    1090             :     }
+    1091             : 
+    1092             :   } else {
+    1093           0 :     ROS_WARN("[%s]: Switch to invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    1094           0 :     res.success = false;
+    1095           0 :     res.message = ("Not a valid estimator type");
+    1096           0 :     return true;
+    1097             :   }
+    1098             : 
+    1099           0 :   res.success = true;
+    1100           0 :   res.message = "Estimator switch successful";
+    1101             : 
+    1102             :   // allow service calllbacks after switch again
+    1103           0 :   callbacks_enabled_ = true;
+    1104             : 
+    1105           0 :   return true;
+    1106             : }
+    1107             : /*//}*/
+    1108             : 
+    1109             : /* //{ callbackToggleServiceCallbacks() */
+    1110          51 : bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1111             : 
+    1112          51 :   if (!sm_->isInitialized()) {
+    1113           0 :     ROS_ERROR("[%s]: service for toggling callbacks is not available before initialization.", getName().c_str());
+    1114           0 :     return false;
+    1115             :   }
+    1116             : 
+    1117          51 :   callbacks_disabled_by_service_ = !req.data;
+    1118             : 
+    1119          51 :   res.success = true;
+    1120          51 :   res.message = (callbacks_disabled_by_service_ ? "Service callbacks disabled" : "Service callbacks enabled");
+    1121             : 
+    1122          51 :   if (callbacks_disabled_by_service_) {
+    1123             : 
+    1124          12 :     ROS_INFO("[%s]: Service callbacks disabled.", getName().c_str());
+    1125             : 
+    1126             :   } else {
+    1127             : 
+    1128          39 :     ROS_INFO("[%s]: Service callbacks enabled", getName().c_str());
+    1129             :   }
+    1130             : 
+    1131          51 :   return true;
+    1132             : }
+    1133             : 
+    1134             : //}
+    1135             : 
+    1136             : 
+    1137             : // --------------------------------------------------------------
+    1138             : // |                        other methods                       |
+    1139             : // --------------------------------------------------------------
+    1140             : //
+    1141             : /*//{ switchToHealthyEstimator() */
+    1142           0 : bool EstimationManager::switchToHealthyEstimator() {
+    1143             : 
+    1144             :   // available estimators should be specified in decreasing priority order in config file
+    1145           0 :   for (auto estimator : estimator_list_) {
+    1146           0 :     if (estimator->isRunning()) {
+    1147           0 :       switchToEstimator(estimator);
+    1148           0 :       return true;
+    1149             :     }
+    1150             :   }
+    1151           0 :   return false;  // no other estimator is running
+    1152             : }
+    1153             : /*//}*/
+    1154             : 
+    1155             : /*//{ switchToEstimator() */
+    1156           0 : void EstimationManager::switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator) {
+    1157             : 
+    1158           0 :   std::scoped_lock lock(mutex_active_estimator_);
+    1159           0 :   ROS_INFO("[%s]: switching estimator from %s to %s", getName().c_str(), active_estimator_->getName().c_str(), target_estimator->getName().c_str());
+    1160           0 :   active_estimator_ = target_estimator;
+    1161           0 :   estimator_switch_count_++;
+    1162           0 : }
+    1163             : /*//}*/
+    1164             : 
+    1165             : /*//{ callFailsafeService() */
+    1166           0 : bool EstimationManager::callFailsafeService() {
+    1167           0 :   std_srvs::Trigger srv_out;
+    1168           0 :   return srvch_failsafe_.call(srv_out);
+    1169             : }
+    1170             : /*//}*/
+    1171             : 
+    1172             : /*//{ getName() */
+    1173        1198 : std::string EstimationManager::getName() const {
+    1174        1198 :   return nodelet_name_;
+    1175             : }
+    1176             : /*//}*/
+    1177             : 
+    1178             : /* loadConfigFile() //{ */
+    1179             : 
+    1180           0 : bool EstimationManager::loadConfigFile(const std::string& file_path) {
+    1181             : 
+    1182           0 :   const std::string name_space = nh_.getNamespace() + "/";
+    1183             : 
+    1184           0 :   ROS_INFO("[%s]: loading '%s' under the namespace '%s'", getName().c_str(), file_path.c_str(), name_space.c_str());
+    1185             : 
+    1186             :   // load the user-requested file
+    1187             :   {
+    1188           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    1189           0 :     int         result  = std::system(command.c_str());
+    1190             : 
+    1191           0 :     if (result != 0) {
+    1192           0 :       ROS_ERROR("[%s]: failed to load '%s'", getName().c_str(), file_path.c_str());
+    1193           0 :       return false;
+    1194             :     }
+    1195             :   }
+    1196             : 
+    1197             :   // load the platform config
+    1198           0 :   if (_platform_config_ != "") {
+    1199           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    1200           0 :     int         result  = std::system(command.c_str());
+    1201             : 
+    1202           0 :     if (result != 0) {
+    1203           0 :       ROS_ERROR("[%s]: failed to load the platform config file '%s'", getName().c_str(), _platform_config_.c_str());
+    1204           0 :       return false;
+    1205             :     }
+    1206             :   }
+    1207             : 
+    1208             :   // load the custom config
+    1209           0 :   if (_custom_config_ != "") {
+    1210           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    1211           0 :     int         result  = std::system(command.c_str());
+    1212             : 
+    1213           0 :     if (result != 0) {
+    1214           0 :       ROS_ERROR("[%s]: failed to load the custom config file '%s'", getName().c_str(), _custom_config_.c_str());
+    1215           0 :       return false;
+    1216             :     }
+    1217             :   }
+    1218             : 
+    1219           0 :   return true;
+    1220             : }
+    1221             : 
+    1222             : //}
+    1223             : 
+    1224             : }  // namespace estimation_manager
+    1225             : 
+    1226             : }  // namespace mrs_uav_managers
+    1227             : 
+    1228             : #include <pluginlib/class_list_macros.h>
+    1229          42 : 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..2f21979eca --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html @@ -0,0 +1,328 @@ + + + + + + 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 + + +
+ 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..86871c9f194a40ca6ab4d87f667d31d9ea1c8b5d GIT binary patch literal 4305 zcmV;?5H9bDP)dWGf&=@ptAr1!tdHt_`rh$jB+%#Ppg-?T%KmxzLPxZS4PyN%OO zXb05(&&>>I05&y@@!ce15nz#{rmiKXIKXoh5R9jAaTpcq%z%wuE6n6_Gy&SXmU&_T zdDeNOI3p*DQAEXbZ3eV)ZKkoJYa>Rn>+2h@LPkE#auoU301E8-b~7E1p-=z1xQCg+_(-6Ur1<*&%i;m8G@2WxF(a*pM%Vh%==xmvE)E(OS;>(B z#LNtV7W}ExH31|r$NQdK%M4$_HDznA7z*}wyMe%GpX9YgK(^H6eCFko<{g=|EO~t> zqz^g`SJD^nS#lah>*=BEDd?@l%$%|D$YoyPaqpRZvqQ&@@%endT-(Aw>-(Secv;?OQ{i0;(3!OKEwk>^73EXoE%s8+4>5A zD-oNB(IlRWeZ~C_O;?uWA;&bof?-Q0T4z0qe7tnF0rSRL+v^ZY#8?vK0cbHa$BgJm zFxMuh(kgIS=90kUlpP+f7Wf+wH*RU{t?(CXMvWhZp+LTMxn^F!SKzX^Wr4@15WQY2 z@cmQ6Q1Id0uhPsj&4uDT7@3Ka=ITQT<||ifX2Te-6Szlc0yM=lO#%;GvOVwu53lKi zO9k$lVJIYK`e?=r2H61%Xu{Z0h1#I`hzi|jzjw^6MH;EVnXg0r0*ka1~wGbG;ZYB|r|wEz;-Sbz%Ia!DVIInj#J2Ino|lRu=$Xl!3+AfR;4C z$qJa*IYVqjXez)oV)QuGOw$mOP37UxIoTwqUUAZH-v5*QV~0e}=p|WVpE`i>npNlI z>s}F~(XL>>h5`*~=;eUMx(j=?$>k*<7PhNtG}i%LSl&|x@qob|FGJX8Dg=pc9neEf zV9coQs?c@AgcSQ{wJ!s1vO;^<&a;0tzMMwVf_*B0PE-l- zny%x(&;TdU@l6Eux*Wl~YXSGj*8*6lEt%{1WCg6XW)UO8T8+^nhU2ee)>l@J+}O27 z0PgyRzBe^UG3#1JK)XKAH|1I>yc!7C=XeIzQ#uC`KNWL@(~jf~I5B`@xoG=>b|Euf z4LP9aVw;g>ngF14ylwaNWg%PY8Il+p@lq0!t*0>3#svT=RW zn3-}w6M1#ajCp5gn(?3=1r!H)W7olp)Z!ortIsQF6`D2sJvK16Gd!N*k!L&Y#A!>e z3WR+&ugft6t@)O!uWAm6|a$zs2NYl8ADMz(U_j4=*ERhAWi z)~=73B7_g)6hfdq3>c@lz4VSLfUYz4;rqepDDDF*G?qs5;A%W{noT1;51ozx<>-AN zaQ}_RSZ@Zek{=@h3QQ%Nj~c^;7)$554BGB?h$We+f1DpMHT@mBb|-uk6$a1gl3ch^U2B!k5%3O;OXb>x z@IG)AA6u}dEm_f5S&v{vfSGl#SJ@)KV1*p1)krjUU?!^V_$OEB*{=QR9~N+daEv04wg;NWU*3oieKQ$d*fnEr zNY_0)Zq5uNPLUbv4Krq-Wco9MDLY;mj6Dr^rc9$Q@~-8=G-KCo;+ZprG;xg#MVuCc zQDk;SdC#mz5|G_`%mI0~&4RQwBmv5ieN@%~@E6$^Xgm|ybMgIG#b$qz{a<9?Zasn( z{fNl^|2cD=5f3XNdG(QPA%hDG7(+^qH0PR^6b3xL%g5{$nF`8;DDbQw9<*}T)so|( zT}w`=ahZZ;sBw*jeX{HQT|}tUh}5hzBlFTGp70s9Gp1_>kf-r+*bQC)@qi1{wYUTT zyf<-e4sw|jISea;H_HOFd!IsbYio5<2gO;eDR51B_TH>1RPfX1#>}GmA(rD9HV4>u zc0D9Vg&E{XKW2v(Gtck?q33+fa3927ijA4geg|ApET(zqZ zk7=ZXdC?HirVby6AC1Z}$UjlXNweiHJBL*PyccSPK-ZX^#l6n(t(Rl!S;uHA7G0=J%RzP=>7cSK6B zGtHpc=X<>O1?U4rMP&~7CO1&xM63@DNj0SY~I)U#M z=Tj`VQVLGTTPVX)l_c+8V@b+mCa(kpe~d1B4p$O9Tt*mS1mlX$oMl8hKSi|#u0ka( z_qdzk1`T&v8(9Qk7ceIpbq5KUu^AdW7tSHRoR>Hy;`y5P3q8ef{jD5*egwjo2z4Ve+U)8+Cm&kms{krrtvTL!wH!KxK}AG!}+dr3m0hg79N9>m?3cYtgYdz1S3c+ ztT3(MuDumRxMf`%<=ElQ0($}iU&{yI0^t*`L(XMQp(3~u2qIji>yz!`$rZD`;_X== zRQ>0Mi!brcAh~@pEq0DCU?R3arBnLW~uG`_=6Wj{xtkxPDd-fNa6qyWGkKM%ITP%Se8i5-(_^q+{$%`FK(f>9H3qf0QC*i76+Lqwh3hfRfL1x8!KG}e zU^%YGui}83%hiv!t&1Ex?2$5U&MOz2W`*e%H(;2VL5t0B6$vCPBM=QPv#$Au$9P^6 zlYVjxuPm1HUDNU=q5O%eW@+J?DO^h=h%|B-&vBhsX;hh6m$g)u6FrSFIY+a_=mGnj8w!i6S6X{lq=FXkTugL_A%Rb^B@Hp+N%7;1koZ71 zVA1a8rZt+Ze3`EQ!TK6R=|CQR# zRd?eYxD2O!tr+9L4g>$JhZzfp}*bghxNZSI4ZkRL&<4rJkvg@*hN4%d= zF8TtxHm0vgkzLx-$kx}))<-<*mg?B0Sim}nglInW@A$KEHex*Wo9-8 z<3-H$DCJ1=wdSJ?>+3KS^0D%8b21%V3^r%rA^+%Jx*T{n9~v-r4F};h9{1mPv*>ka zsKN>wdwDY@B=j2^f8OkWw5mmL@|QF&MPNJx~jh&vh{&>-;dsm2kr>?Aeh&K+!SAg(^m(JeL9uO_Cn zm3#dmmQ-;%DHGu^2vZzwb4^|$&ywW$-v0qW?f`pgO-FZ1OfOpn^a z1{&3x7=^|R@M;A8u}R-jXg%`4;OjX92WoHw)nLr_L%IH3e1{eeG0+SAoVX zrO9)@;3SFvHYcVI;^8J&F7OiTalxIaUsTsj8`X@lk!e^yGx6DjmDLG3&<0qXs@ zCM!??U&?g=0k9;T&n_UbBNZ;Ecvk6}-gqH6Kjqqqs09VkhEb%^cuUL^4QzOE#zq~f z%}ZfGfQ#Cj2Q0ga4tpu}r`X6%BuF-|6zx?_aa$bWuY4qacIJvC7P^L-PB)8iTw7@j zrJ>xazc5997<=8b33kAp?A(z!2gRfshkynVPD8^6Du5i0ZvpK1Md) z!=_{wfB0}Ci{`=<$45m?Q(Tu|I2go5N9cL$S(BZ3M_o0pH~95bN4_DetM?RsF}l8R zR2^xqo4F=X@g%Fs?ChV3#OR|O@9XLxUXE)wnD)2Cd!pe~y*sme#1bGb@n7Kq0KB7V zTc>=uf-R_URL9v9awrzWfPD%^g-DIxrb2MZX&ewgOoh4zfeKX(mQ + + + + + + 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:618571.8 %
Date:2023-12-06 07:59:45Functions: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)916
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)916
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const916
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const1395
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const1832
mrs_uav_managers::Estimator::getMaxFlightZ() const4961
mrs_uav_managers::Estimator::isStarted() const5785
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)92050
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)92089
mrs_uav_managers::Estimator::isReady() const99302
mrs_uav_managers::Estimator::getName[abi:cxx11]() const170800
mrs_uav_managers::Estimator::isMitigatingJump() const190487
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const196154
mrs_uav_managers::Estimator::publishDiagnostics() const282062
mrs_uav_managers::Estimator::isError() const352613
mrs_uav_managers::Estimator::isRunning() const439971
mrs_uav_managers::Estimator::isInitialized() const991749
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const2763837
mrs_uav_managers::Estimator::getCurrentSmState() const2960531
+
+
+ + + +
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..95e463b16a --- /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:618571.8 %
Date:2023-12-06 07:59:45Functions: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)916
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)92050
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)92089
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&)916
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const196154
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const1395
mrs_uav_managers::Estimator::getMaxFlightZ() const4961
mrs_uav_managers::Estimator::isInitialized() const991749
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const1832
mrs_uav_managers::Estimator::isMitigatingJump() const190487
mrs_uav_managers::Estimator::getCurrentSmState() const2960531
mrs_uav_managers::Estimator::publishDiagnostics() const282062
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const916
mrs_uav_managers::Estimator::getName[abi:cxx11]() const170800
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const352613
mrs_uav_managers::Estimator::isReady() const99302
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const2763837
mrs_uav_managers::Estimator::isRunning() const439971
mrs_uav_managers::Estimator::isStarted() const5785
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..1fc9099127 --- /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:618571.8 %
Date:2023-12-06 07:59:45Functions: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         916 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10         916 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14         916 :   previous_sm_state_ = getCurrentSmState();
+      15         916 :   setCurrentSmState(new_state);
+      16             : 
+      17         916 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18         916 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23     2763837 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24     2763837 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29      991749 : bool Estimator::isInitialized() const {
+      30      991749 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35       99302 : bool Estimator::isReady() const {
+      36       99302 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41        5785 : bool Estimator::isStarted() const {
+      42        5785 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47      439971 : bool Estimator::isRunning() const {
+      48      439971 :   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      352613 : bool Estimator::isError() const {
+      60      352613 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65     2960531 : SMStates_t Estimator::getCurrentSmState() const {
+      66     2960531 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71         916 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72         916 :   std::scoped_lock lock(mutex_current_state_);
+      73         916 :   current_sm_state_ = new_state;
+      74         916 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78        1832 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79        1832 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84         916 : std::string Estimator::getCurrentSmStateString(void) const {
+      85        1832 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90      190487 : bool Estimator::isMitigatingJump(void) const {
+      91      190487 :   return is_mitigating_jump_;
+      92             : }
+      93             : /*//}*/
+      94             : 
+      95             : /*//{ getName() */
+      96      170800 : std::string Estimator::getName(void) const {
+      97      170800 :   return name_;
+      98             : }
+      99             : /*//}*/
+     100             : 
+     101             : /*//{ getPrintName() */
+     102        1395 : std::string Estimator::getPrintName(void) const {
+     103        2790 :   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      196154 : std::string Estimator::getFrameId(void) const {
+     115      196154 :   return ns_frame_id_;
+     116             : }
+     117             : /*//}*/
+     118             : 
+     119             : /*//{ getMaxFlightZ() */
+     120        4961 : double Estimator::getMaxFlightZ(void) const {
+     121        4961 :   return max_flight_z_;
+     122             : }
+     123             : /*//}*/
+     124             : 
+     125             : /*//{ publishDiagnostics() */
+     126      282062 : void Estimator::publishDiagnostics() const {
+     127             : 
+     128      282062 :   if (!ch_->debug_topics.diag) {
+     129      282048 :     return;
+     130             :   }
+     131             : 
+     132           0 :   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       92089 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     153             : 
+     154      183538 :   geometry_msgs::Vector3Stamped acc_stamped;
+     155       91939 :   acc_stamped.vector = input_msg->control_acceleration;
+     156       91898 :   acc_stamped.header = input_msg->header;
+     157      182911 :   return getAccGlobal(acc_stamped, hdg);
+     158             : }
+     159             : 
+     160       92050 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     161             : 
+     162             :   // untilt the desired acceleration vector
+     163      183477 :   geometry_msgs::Vector3Stamped des_acc;
+     164       91803 :   geometry_msgs::Vector3        des_acc_untilted;
+     165       91879 :   des_acc.vector.x        = acc_stamped.vector.x;
+     166       91879 :   des_acc.vector.y        = acc_stamped.vector.y;
+     167       91879 :   des_acc.vector.z        = acc_stamped.vector.z;
+     168       91879 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     169       91932 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     170      183913 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     171       92098 :   if (response_acc) {
+     172       92098 :     des_acc_untilted.x = response_acc.value().vector.x;
+     173       92098 :     des_acc_untilted.y = response_acc.value().vector.y;
+     174       92098 :     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       92098 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     182             : 
+     183      182881 :   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..0542f334157864cbe9b6e41d42d270c218eb0f56 GIT binary patch literal 799 zcmV+)1K|9LP)xi7#@*z z89sxuW_h=)(VdW4bFnr6Yu42TYmzd?nk4gn))ynZL%a3$-(*d2{|MGc*@|M^hbT}} zsBl@R$|bCD`hQ=)mNoD1gFzL!82gfShuMd)ZZ<2Z+1pyMMoiR_NkFAu6cl|k1K=(5 zbeiiB6l$v^TWAd?`navo4GWz02Ka_G63M>dI0|dV*VqYZZP!?`rl}3UT9WK*U!$ZG zYnIahtT|8P5zZ%jcZOUE6AZwb_jPPzvbdgQr`s8}Ss9?6VO@>+8trPJulp{1zpo`n zQb{vYvhMO4gEiytHiuK$OiE@cH6pr_M$Uo}C^V$pJldAB6FWBp-C*CrfCM^C5!lyg zqegCOInWU)&QzuX3Z*s=nX`t-W?VW;dAmyE_2|BN1VOP^^IIN>*|t446Su^2&xj>u zhbmdr>Err^Z%(&(CBZMHkqnFi(dMJ+!-@N3c@BT69la^!cAWfskNMaSvBKlq4Kp~Z z5d!@K%9a`EN1$c+P2iSJ&ZxT~u+UgzJ#bCW(bL@ddoFS0O;6NFN3Qy5pz73K2-Lbv zCnn^y-S|(=H!wfiMhr|TKk#}Fi}3TSi1a0;k>nxFjp|yGiiYk2$LRL@#jq(_#8_Sn zr)PNU$}e)3u|A`#DVUAOUw-QDRWt;`z&3e1m^_T;IYxr1FRbxY&jH`qa*5^jQ_qiZ dVh{3dg + + + + + + 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:2023-12-06 07:59:45Functions: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&) const35
mrs_uav_managers::AglEstimator::publishAglHeight() const43361
mrs_uav_managers::AglEstimator::publishCovariance() const43361
+
+
+ + + +
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..47c42ae25a --- /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:2023-12-06 07:59:45Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const43361
mrs_uav_managers::AglEstimator::publishCovariance() const43361
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const35
+
+
+ + + +
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..dadbfbdb04 --- /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:2023-12-06 07:59:45Functions: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       43361 : void AglEstimator::publishAglHeight() const {
+       8       43361 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9       43361 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13       43361 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15       43361 :   if (!ch_->debug_topics.covariance) {
+      16       43361 :     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          35 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26          35 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27          35 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29          35 :   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          35 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35          35 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36          35 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37          35 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38          35 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39          35 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40          35 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41          35 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42          35 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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          35 :   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..5f12fc1b7d --- /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:2023-12-06 07:59:45Functions: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..ab3cafb9c5 --- /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:2023-12-06 07:59:45Functions: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..21bc38a60c --- /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:2023-12-06 07:59:45Functions: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..3549e6092d --- /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:2023-12-06 07:59:45Functions: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..2ced03b2ef --- /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:2023-12-06 07:59:45Functions: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..ab0480089b --- /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:2023-12-06 07:59:45Functions: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..9ab17f3996 --- /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:2023-12-06 07:59:45Functions: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&) const42
mrs_uav_managers::StateEstimator::getInnovation() const46422
mrs_uav_managers::StateEstimator::getPoseCovariance() const46422
mrs_uav_managers::StateEstimator::getTwistCovariance() const46422
mrs_uav_managers::StateEstimator::getUavState()51228
mrs_uav_managers::StateEstimator::publishOdom() const52163
mrs_uav_managers::StateEstimator::publishUavState() const52163
mrs_uav_managers::StateEstimator::publishCovariance() const52163
mrs_uav_managers::StateEstimator::publishInnovation() const52163
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const96664
+
+
+ + + +
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..fa3e7dbe9f --- /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:2023-12-06 07:59:45Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::getUavState()51228
mrs_uav_managers::StateEstimator::publishOdom() const52163
mrs_uav_managers::StateEstimator::getInnovation() const46422
mrs_uav_managers::StateEstimator::publishUavState() const52163
mrs_uav_managers::StateEstimator::getPoseCovariance() const46422
mrs_uav_managers::StateEstimator::publishCovariance() const52163
mrs_uav_managers::StateEstimator::publishInnovation() const52163
mrs_uav_managers::StateEstimator::getTwistCovariance() const46422
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const42
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const96664
+
+
+ + + +
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..24145ef522 --- /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:2023-12-06 07:59:45Functions: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       51228 : std::optional<mrs_msgs::UavState> StateEstimator::getUavState() {
+       9             : 
+      10       51228 :   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      102449 :   return mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      16             : }
+      17             : /*//}*/
+      18             : 
+      19             : /*//{ getInnovation() */
+      20       46422 : nav_msgs::Odometry StateEstimator::getInnovation() const {
+      21       46422 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      22             : }
+      23             : /*//}*/
+      24             : 
+      25             : /*//{ getPoseCovariance() */
+      26       46422 : std::vector<double> StateEstimator::getPoseCovariance() const {
+      27       46422 :   return mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_.values);
+      28             : }
+      29             : /*//}*/
+      30             : 
+      31             : /*//{ getTwistCovariance() */
+      32       46422 : std::vector<double> StateEstimator::getTwistCovariance() const {
+      33       46422 :   return mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_.values);
+      34             : }
+      35             : /*//}*/
+      36             : 
+      37             : /*//{ publishUavState() */
+      38       52163 : void StateEstimator::publishUavState() const {
+      39             : 
+      40       52163 :   if (!ch_->debug_topics.state) {
+      41           0 :     return;
+      42             :   }
+      43             : 
+      44      104326 :   auto uav_state = mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      45       52163 :   ph_uav_state_.publish(uav_state);
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ publishOdom() */
+      50       52163 : void StateEstimator::publishOdom() const {
+      51             : 
+      52      104326 :   auto odom = mrs_lib::get_mutexed(mtx_odom_, odom_);
+      53       52163 :   ph_odom_.publish(odom);
+      54       52163 : }
+      55             : /*//}*/
+      56             : 
+      57             : /*//{ publishCovariance() */
+      58       52163 : void StateEstimator::publishCovariance() const {
+      59             : 
+      60       52163 :   if (!ch_->debug_topics.covariance) {
+      61       52163 :     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       52163 : void StateEstimator::publishInnovation() const {
+      73             : 
+      74       52163 :   if (!ch_->debug_topics.innovation) {
+      75       52163 :     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       96664 : std::optional<geometry_msgs::Quaternion> StateEstimator::rotateQuaternionByHeading(const geometry_msgs::Quaternion& q, const double hdg) const {
+      85             : 
+      86             :   try {
+      87       96664 :     tf2::Quaternion tf2_q = mrs_lib::AttitudeConverter(q);
+      88             : 
+      89             :     // Obtain heading from quaternion
+      90       96554 :     double q_hdg = 0;
+      91       96554 :     q_hdg        = mrs_lib::AttitudeConverter(q).getHeading();
+      92             : 
+      93             :     // Build rotation matrix from difference between new heading and quaternion heading
+      94       96278 :     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       96155 :     geometry_msgs::Quaternion q_new = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_q);
+      98       95834 :     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          42 : bool StateEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+     109             : 
+     110          42 :   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          42 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+     116          42 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+     117          42 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+     118          42 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+     119          42 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+     120          42 :   ph_->param_loader->loadParam("requires/position", requires_position);
+     121          42 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+     122          42 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+     123          42 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+     124             : 
+     125          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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          42 :   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..6beed42bb3 --- /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:39161363.8 %
Date:2023-12-06 07:59:45Functions:374778.7 %
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 +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
<unnamed>62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
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..5466e9c034 --- /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:39161363.8 %
Date:2023-12-06 07:59:45Functions:374778.7 %
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 +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
<unnamed>62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 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..35500f8254 --- /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:39161363.8 %
Date:2023-12-06 07:59:45Functions:374778.7 %
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 +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
<unnamed>62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 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..95b0e74c0b --- /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:39161363.8 %
Date:2023-12-06 07:59:45Functions:374778.7 %
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 +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
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..40c79053b9 --- /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:39161363.8 %
Date:2023-12-06 07:59:45Functions:374778.7 %
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 +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 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..5453d7da33 --- /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:39161363.8 %
Date:2023-12-06 07:59:45Functions:374778.7 %
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 +
62.5%62.5%
+ 		62.5 %330 / 52875.0 %18 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 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..520046b785 --- /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:20225379.8 %
Date:2023-12-06 07:59:45Functions: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()42
mrs_uav_managers::gain_manager::GainManager::onInit()42
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)42
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)676
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&)924
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)6906
+
+
+ + + +
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..959f43536c --- /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:20225379.8 %
Date:2023-12-06 07:59:45Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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&)924
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&)676
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)6906
mrs_uav_managers::gain_manager::GainManager::onInit()42
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)42
+
+
+ + + +
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..29dce5132c --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.html @@ -0,0 +1,720 @@ + + + + + + + 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:20225379.8 %
Date:2023-12-06 07:59:45Functions: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             :   bool            is_initialized_ = false;
+      56             : 
+      57             :   // | ----------------------- parameters ----------------------- |
+      58             : 
+      59             :   std::vector<std::string> _current_state_estimators_;
+      60             : 
+      61             :   std::vector<std::string>       _gain_names_;
+      62             :   std::map<std::string, Gains_t> _gains_;
+      63             : 
+      64             :   std::map<std::string, std::vector<std::string>> _map_type_allowed_gains_;
+      65             :   std::map<std::string, std::string>              _map_type_default_gains_;
+      66             :   ;
+      67             : 
+      68             :   // | --------------------- service clients -------------------- |
+      69             : 
+      70             :   ros::ServiceClient service_client_set_gains_;
+      71             : 
+      72             :   // | ----------------------- subscribers ---------------------- |
+      73             : 
+      74             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>     sh_estimation_diag_;
+      75             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+      76             : 
+      77             :   // | --------------------- gain management -------------------- |
+      78             : 
+      79             :   bool setGains(std::string gains_name);
+      80             : 
+      81             :   ros::ServiceServer service_server_set_gains_;
+      82             :   bool               callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+      83             : 
+      84             :   std::string last_estimator_name_;
+      85             :   std::mutex  mutex_last_estimator_name_;
+      86             : 
+      87             :   void       timerGainManagement(const ros::TimerEvent& event);
+      88             :   ros::Timer timer_gain_management_;
+      89             :   double     _gain_management_rate_;
+      90             : 
+      91             :   std::string current_gains_;
+      92             :   std::mutex  mutex_current_gains_;
+      93             : 
+      94             :   // | ------------------ diagnostics publisher ----------------- |
+      95             : 
+      96             :   mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics> ph_diagnostics_;
+      97             : 
+      98             :   void       timerDiagnostics(const ros::TimerEvent& event);
+      99             :   ros::Timer timer_diagnostics_;
+     100             :   double     _diagnostics_rate_;
+     101             : 
+     102             :   // | ------------------------ profiler ------------------------ |
+     103             : 
+     104             :   mrs_lib::Profiler profiler_;
+     105             :   bool              _profiler_enabled_ = false;
+     106             : 
+     107             :   // | ------------------- scope timer logger ------------------- |
+     108             : 
+     109             :   bool                                       scope_timer_enabled_ = false;
+     110             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     111             : 
+     112             :   // | ------------------------- helpers ------------------------ |
+     113             : 
+     114             :   bool stringInVector(const std::string& value, const std::vector<std::string>& vector);
+     115             : };
+     116             : 
+     117             : //}
+     118             : 
+     119             : /* //{ onInit() */
+     120             : 
+     121          42 : void GainManager::onInit() {
+     122             : 
+     123          84 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     124             : 
+     125          42 :   ros::Time::waitForValid();
+     126             : 
+     127          42 :   ROS_INFO("[GainManager]: initializing");
+     128             : 
+     129             :   // | ------------------------- params ------------------------- |
+     130             : 
+     131          84 :   mrs_lib::ParamLoader param_loader(nh_, "GainManager");
+     132             : 
+     133          84 :   std::string custom_config_path;
+     134          84 :   std::string platform_config_path;
+     135             : 
+     136          42 :   param_loader.loadParam("custom_config", custom_config_path);
+     137          42 :   param_loader.loadParam("platform_config", platform_config_path);
+     138             : 
+     139          42 :   if (custom_config_path != "") {
+     140          42 :     param_loader.addYamlFile(custom_config_path);
+     141             :   }
+     142             : 
+     143          42 :   if (platform_config_path != "") {
+     144          42 :     param_loader.addYamlFile(platform_config_path);
+     145             :   }
+     146             : 
+     147          42 :   param_loader.addYamlFileFromParam("private_config");
+     148          42 :   param_loader.addYamlFileFromParam("public_config");
+     149          42 :   param_loader.addYamlFileFromParam("public_gains");
+     150             : 
+     151          84 :   const std::string yaml_prefix = "mrs_uav_managers/gain_manager/";
+     152             : 
+     153             :   // params passed from the launch file are not prefixed
+     154          42 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     155             : 
+     156          42 :   param_loader.loadParam(yaml_prefix + "gains", _gain_names_);
+     157             : 
+     158          42 :   param_loader.loadParam(yaml_prefix + "estimator_types", _current_state_estimators_);
+     159             : 
+     160          42 :   param_loader.loadParam(yaml_prefix + "rate", _gain_management_rate_);
+     161          42 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     162             : 
+     163             :   // | ------------------- scope timer logger ------------------- |
+     164             : 
+     165          42 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     166         126 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     167          42 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     168             : 
+     169          42 :   std::vector<std::string>::iterator it;
+     170             : 
+     171             :   // loading gain_names
+     172         126 :   for (it = _gain_names_.begin(); it != _gain_names_.end(); ++it) {
+     173             : 
+     174          84 :     ROS_INFO_STREAM("[GainManager]: loading gains '" << *it << "'");
+     175             : 
+     176          84 :     Gains_t new_gains;
+     177             : 
+     178          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kp", new_gains.kpxy);
+     179          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kv", new_gains.kvxy);
+     180          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/ka", new_gains.kaxy);
+     181          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib", new_gains.kibxy);
+     182          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw", new_gains.kiwxy);
+     183          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib_lim", new_gains.kibxy_lim);
+     184          84 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw_lim", new_gains.kiwxy_lim);
+     185             : 
+     186          84 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kp", new_gains.kpz);
+     187          84 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kv", new_gains.kvz);
+     188          84 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ka", new_gains.kaz);
+     189             : 
+     190          84 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_roll_pitch", new_gains.kqrp);
+     191          84 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_yaw", new_gains.kqy);
+     192             : 
+     193          84 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km", new_gains.km);
+     194          84 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km_lim", new_gains.km_lim);
+     195             : 
+     196          84 :     _gains_.insert(std::pair<std::string, Gains_t>(*it, new_gains));
+     197             :   }
+     198             : 
+     199             :   // loading the allowed gains lists
+     200         336 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     201             : 
+     202         294 :     std::vector<std::string> temp_vector;
+     203         294 :     param_loader.loadParam(yaml_prefix + "allowed_gains/" + *it, temp_vector);
+     204             : 
+     205         294 :     std::vector<std::string>::iterator it2;
+     206         882 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     207         588 :       if (!stringInVector(*it2, _gain_names_)) {
+     208           0 :         ROS_ERROR("[GainManager]: the element '%s' of %s/allowed_gains is not a valid gain!", it2->c_str(), it->c_str());
+     209           0 :         ros::shutdown();
+     210             :       }
+     211             :     }
+     212             : 
+     213         294 :     _map_type_allowed_gains_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     214             :   }
+     215             : 
+     216             :   // loading the default gains
+     217         336 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     218             : 
+     219         294 :     std::string temp_str;
+     220         294 :     param_loader.loadParam(yaml_prefix + "default_gains/" + *it, temp_str);
+     221             : 
+     222         294 :     if (!stringInVector(temp_str, _map_type_allowed_gains_.at(*it))) {
+     223           0 :       ROS_ERROR("[GainManager]: the element '%s' of %s/allowed_gains is not a valid gain!", temp_str.c_str(), it->c_str());
+     224           0 :       ros::shutdown();
+     225             :     }
+     226             : 
+     227         294 :     _map_type_default_gains_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     228             :   }
+     229             : 
+     230          42 :   ROS_INFO("[GainManager]: done loading dynamical params");
+     231             : 
+     232          42 :   current_gains_       = "";
+     233          42 :   last_estimator_name_ = "";
+     234             : 
+     235             :   // | ------------------------ services ------------------------ |
+     236             : 
+     237          42 :   service_server_set_gains_ = nh_.advertiseService("set_gains_in", &GainManager::callbackSetGains, this);
+     238             : 
+     239          42 :   service_client_set_gains_ = nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_gains_out");
+     240             : 
+     241             :   // | ----------------------- subscribers ---------------------- |
+     242             : 
+     243          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+     244          42 :   shopts.nh                 = nh_;
+     245          42 :   shopts.node_name          = "GainManager";
+     246          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     247          42 :   shopts.threadsafe         = true;
+     248          42 :   shopts.autostart          = true;
+     249          42 :   shopts.queue_size         = 10;
+     250          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     251             : 
+     252          42 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     253          42 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     254             : 
+     255             :   // | ----------------------- publishers ----------------------- |
+     256             : 
+     257          42 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     258             : 
+     259             :   // | ------------------------- timers ------------------------- |
+     260             : 
+     261          42 :   timer_gain_management_ = nh_.createTimer(ros::Rate(_gain_management_rate_), &GainManager::timerGainManagement, this);
+     262          42 :   timer_diagnostics_     = nh_.createTimer(ros::Rate(_diagnostics_rate_), &GainManager::timerDiagnostics, this);
+     263             : 
+     264             :   // | ------------------------ profiler ------------------------ |
+     265             : 
+     266          42 :   profiler_ = mrs_lib::Profiler(nh_, "GainManager", _profiler_enabled_);
+     267             : 
+     268             :   // | ----------------------- finish init ---------------------- |
+     269             : 
+     270          42 :   if (!param_loader.loadedSuccessfully()) {
+     271           0 :     ROS_ERROR("[GainManager]: could not load all parameters!");
+     272           0 :     ros::shutdown();
+     273             :   }
+     274             : 
+     275          42 :   is_initialized_ = true;
+     276             : 
+     277          42 :   ROS_INFO("[GainManager]: initialized");
+     278             : 
+     279          42 :   ROS_DEBUG("[GainManager]: debug output is enabled");
+     280          42 : }
+     281             : 
+     282             : //}
+     283             : 
+     284             : // --------------------------------------------------------------
+     285             : // |                           methods                          |
+     286             : // --------------------------------------------------------------
+     287             : 
+     288             : /* setGains() //{ */
+     289             : 
+     290          42 : bool GainManager::setGains(std::string gains_name) {
+     291             : 
+     292          42 :   std::map<std::string, Gains_t>::iterator it;
+     293          42 :   it = _gains_.find(gains_name);
+     294             : 
+     295          42 :   if (it == _gains_.end()) {
+     296           0 :     ROS_WARN("[GainManager]: can not set gains for '%s', the mode is not on a list!", gains_name.c_str());
+     297           0 :     return false;
+     298             :   }
+     299             : 
+     300          84 :   dynamic_reconfigure::Config          conf;
+     301          84 :   dynamic_reconfigure::DoubleParameter param;
+     302             : 
+     303          42 :   param.name  = "kpxy";
+     304          42 :   param.value = it->second.kpxy;
+     305          42 :   conf.doubles.push_back(param);
+     306             : 
+     307          42 :   param.name  = "kvxy";
+     308          42 :   param.value = it->second.kvxy;
+     309          42 :   conf.doubles.push_back(param);
+     310             : 
+     311          42 :   param.name  = "kaxy";
+     312          42 :   param.value = it->second.kaxy;
+     313          42 :   conf.doubles.push_back(param);
+     314             : 
+     315          42 :   param.name  = "kq_roll_pitch";
+     316          42 :   param.value = it->second.kqrp;
+     317          42 :   conf.doubles.push_back(param);
+     318             : 
+     319          42 :   param.name  = "kibxy";
+     320          42 :   param.value = it->second.kibxy;
+     321          42 :   conf.doubles.push_back(param);
+     322             : 
+     323          42 :   param.name  = "kiwxy";
+     324          42 :   param.value = it->second.kiwxy;
+     325          42 :   conf.doubles.push_back(param);
+     326             : 
+     327          42 :   param.name  = "kibxy_lim";
+     328          42 :   param.value = it->second.kibxy_lim;
+     329          42 :   conf.doubles.push_back(param);
+     330             : 
+     331          42 :   param.name  = "kiwxy_lim";
+     332          42 :   param.value = it->second.kiwxy_lim;
+     333          42 :   conf.doubles.push_back(param);
+     334             : 
+     335          42 :   param.name  = "kpz";
+     336          42 :   param.value = it->second.kpz;
+     337          42 :   conf.doubles.push_back(param);
+     338             : 
+     339          42 :   param.name  = "kvz";
+     340          42 :   param.value = it->second.kvz;
+     341          42 :   conf.doubles.push_back(param);
+     342             : 
+     343          42 :   param.name  = "kaz";
+     344          42 :   param.value = it->second.kaz;
+     345          42 :   conf.doubles.push_back(param);
+     346             : 
+     347          42 :   param.name  = "kq_yaw";
+     348          42 :   param.value = it->second.kqy;
+     349          42 :   conf.doubles.push_back(param);
+     350             : 
+     351          42 :   param.name  = "km";
+     352          42 :   param.value = it->second.km;
+     353          42 :   conf.doubles.push_back(param);
+     354             : 
+     355          42 :   param.name  = "km_lim";
+     356          42 :   param.value = it->second.km_lim;
+     357          42 :   conf.doubles.push_back(param);
+     358             : 
+     359          84 :   dynamic_reconfigure::ReconfigureRequest  srv_req;
+     360          84 :   dynamic_reconfigure::ReconfigureResponse srv_resp;
+     361             : 
+     362          42 :   srv_req.config = conf;
+     363             : 
+     364          84 :   dynamic_reconfigure::Reconfigure reconf;
+     365          42 :   reconf.request = srv_req;
+     366             : 
+     367          42 :   ROS_INFO_THROTTLE(1.0, "[GainManager]: setting up gains for '%s'", gains_name.c_str());
+     368             : 
+     369          42 :   bool res = service_client_set_gains_.call(reconf);
+     370             : 
+     371          42 :   if (!res) {
+     372             : 
+     373           0 :     ROS_WARN_THROTTLE(1.0, "[GainManager]: the service for setting gains has failed!");
+     374           0 :     return false;
+     375             : 
+     376             :   } else {
+     377             : 
+     378          42 :     mrs_lib::set_mutexed(mutex_current_gains_, gains_name, current_gains_);
+     379             : 
+     380          42 :     return true;
+     381             :   }
+     382             : }
+     383             : 
+     384             : //}
+     385             : 
+     386             : // --------------------------------------------------------------
+     387             : // |                          callbacks                         |
+     388             : // --------------------------------------------------------------
+     389             : 
+     390             : // | -------------------- service callbacks ------------------- |
+     391             : 
+     392             : /* //{ callbackSetGains() */
+     393             : 
+     394           0 : bool GainManager::callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+     395             : 
+     396           0 :   if (!is_initialized_) {
+     397           0 :     return false;
+     398             :   }
+     399             : 
+     400           0 :   std::stringstream ss;
+     401             : 
+     402           0 :   if (!sh_estimation_diag_.hasMsg()) {
+     403             : 
+     404           0 :     ss << "missing estimation diagnostics";
+     405             : 
+     406           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     407             : 
+     408           0 :     res.message = ss.str();
+     409           0 :     res.success = false;
+     410           0 :     return true;
+     411             :   }
+     412             : 
+     413           0 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     414             : 
+     415           0 :   if (!stringInVector(req.value, _gain_names_)) {
+     416             : 
+     417           0 :     ss << "the gains '" << req.value.c_str() << "' do not exist (in the GainManager's config)";
+     418             : 
+     419           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     420             : 
+     421           0 :     res.message = ss.str();
+     422           0 :     res.success = false;
+     423           0 :     return true;
+     424             :   }
+     425             : 
+     426           0 :   if (!stringInVector(req.value, _map_type_allowed_gains_.at(estimation_diagnostics.current_state_estimator))) {
+     427             : 
+     428           0 :     ss << "the gains '" << req.value.c_str() << "' are not allowed given the current state estimator";
+     429             : 
+     430           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     431             : 
+     432           0 :     res.message = ss.str();
+     433           0 :     res.success = false;
+     434           0 :     return true;
+     435             :   }
+     436             : 
+     437             :   // try to set the gains
+     438           0 :   if (!setGains(req.value)) {
+     439             : 
+     440           0 :     ss << "the Se3Controller could not set the gains";
+     441             : 
+     442           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     443             : 
+     444           0 :     res.message = ss.str();
+     445           0 :     res.success = false;
+     446           0 :     return true;
+     447             : 
+     448             :   } else {
+     449             : 
+     450           0 :     ss << "the gains '" << req.value.c_str() << "' are set";
+     451             : 
+     452           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     453             : 
+     454           0 :     res.message = ss.str();
+     455           0 :     res.success = true;
+     456           0 :     return true;
+     457             :   }
+     458             : }
+     459             : 
+     460             : //}
+     461             : 
+     462             : // --------------------------------------------------------------
+     463             : // |                           timers                           |
+     464             : // --------------------------------------------------------------
+     465             : 
+     466             : /* gainManagementTimer() //{ */
+     467             : 
+     468        6906 : void GainManager::timerGainManagement(const ros::TimerEvent& event) {
+     469             : 
+     470        6906 :   if (!is_initialized_) {
+     471        2119 :     return;
+     472             :   }
+     473             : 
+     474       13812 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("gainManagementTimer", _gain_management_rate_, 0.01, event);
+     475       13812 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::gainManagementTimer", scope_timer_logger_, scope_timer_enabled_);
+     476             : 
+     477        6906 :   if (!sh_estimation_diag_.hasMsg()) {
+     478        2119 :     return;
+     479             :   }
+     480             : 
+     481        4787 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     482             : 
+     483        4787 :   if (!sh_control_manager_diag_.hasMsg()) {
+     484           0 :     return;
+     485             :   }
+     486             : 
+     487        9574 :   auto control_manager_diagnostics = *sh_estimation_diag_.getMsg();
+     488             : 
+     489        9574 :   auto current_gains       = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     490        4787 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     491             : 
+     492             :   // | --- automatically set _gains_ when currrent state estimator changes -- |
+     493        4787 :   if (estimation_diagnostics.current_state_estimator != last_estimator_name) {
+     494             : 
+     495          42 :     ROS_INFO_THROTTLE(1.0, "[GainManager]: the state estimator has changed! %s -> %s", last_estimator_name_.c_str(),
+     496             :                       estimation_diagnostics.current_state_estimator.c_str());
+     497             : 
+     498          42 :     std::map<std::string, std::string>::iterator it;
+     499          42 :     it = _map_type_default_gains_.find(estimation_diagnostics.current_state_estimator);
+     500             : 
+     501          42 :     if (it == _map_type_default_gains_.end()) {
+     502             : 
+     503           0 :       ROS_WARN_THROTTLE(1.0, "[GainManager]: the state estimator '%s' was not specified in the gain_manager's config!",
+     504             :                         estimation_diagnostics.current_state_estimator.c_str());
+     505             : 
+     506             :     } else {
+     507             : 
+     508             :       // if the current gains are within the allowed estimator types, do nothing
+     509          42 :       if (stringInVector(current_gains, _map_type_allowed_gains_.at(estimation_diagnostics.current_state_estimator))) {
+     510             : 
+     511           0 :         last_estimator_name = estimation_diagnostics.current_state_estimator;
+     512             : 
+     513             :         // else, try to set the default gains
+     514             :       } else {
+     515             : 
+     516          42 :         ROS_WARN_THROTTLE(1.0, "[GainManager]: the current gains '%s' are not within the allowed gains for '%s'", current_gains.c_str(),
+     517             :                           estimation_diagnostics.current_state_estimator.c_str());
+     518             : 
+     519          42 :         if (setGains(it->second)) {
+     520             : 
+     521          42 :           last_estimator_name = estimation_diagnostics.current_state_estimator;
+     522             : 
+     523          42 :           ROS_INFO_THROTTLE(1.0, "[GainManager]: gains set to default: '%s'", it->second.c_str());
+     524             : 
+     525             :         } else {
+     526             : 
+     527           0 :           ROS_WARN_THROTTLE(1.0, "[GainManager]: could not set gains!");
+     528             :         }
+     529             :       }
+     530             :     }
+     531             :   }
+     532             : 
+     533        4787 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     534             : }
+     535             : 
+     536             : //}
+     537             : 
+     538             : /* dignosticsTimer() //{ */
+     539             : 
+     540         676 : void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
+     541             : 
+     542         676 :   if (!is_initialized_) {
+     543         208 :     return;
+     544             :   }
+     545             : 
+     546        1352 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     547        1352 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     548             : 
+     549         676 :   if (!sh_estimation_diag_.hasMsg()) {
+     550         208 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do constraint management, missing estimator diagnostics!");
+     551         208 :     return;
+     552             :   }
+     553             : 
+     554         468 :   if (!sh_control_manager_diag_.hasMsg()) {
+     555           0 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do constraint management, missing control manager diagnostics!");
+     556           0 :     return;
+     557             :   }
+     558             : 
+     559         936 :   auto estimation_diagnostics = *sh_estimation_diag_.getMsg();
+     560             : 
+     561         936 :   auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     562             : 
+     563         936 :   mrs_msgs::GainManagerDiagnostics diagnostics;
+     564             : 
+     565         468 :   diagnostics.stamp        = ros::Time::now();
+     566         468 :   diagnostics.current_name = current_gains;
+     567         468 :   diagnostics.loaded       = _gain_names_;
+     568             : 
+     569             :   // get the available gains
+     570             :   {
+     571         468 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     572         468 :     it = _map_type_allowed_gains_.find(estimation_diagnostics.current_state_estimator);
+     573             : 
+     574         468 :     if (it == _map_type_allowed_gains_.end()) {
+     575           0 :       ROS_WARN_THROTTLE(1.0, "[GainManager]: the estimator name '%s' was not specified in the gain_manager's config!",
+     576             :                         estimation_diagnostics.current_state_estimator.c_str());
+     577             :     } else {
+     578         468 :       diagnostics.available = it->second;
+     579             :     }
+     580             :   }
+     581             : 
+     582             :   // get the current gain values
+     583             :   {
+     584         468 :     std::map<std::string, Gains_t>::iterator it;
+     585         468 :     it = _gains_.find(current_gains);
+     586             : 
+     587         468 :     diagnostics.current_values.kpxy = it->second.kpxy;
+     588         468 :     diagnostics.current_values.kvxy = it->second.kvxy;
+     589         468 :     diagnostics.current_values.kaxy = it->second.kaxy;
+     590             : 
+     591         468 :     diagnostics.current_values.kqrp = it->second.kqrp;
+     592             : 
+     593         468 :     diagnostics.current_values.kibxy     = it->second.kibxy;
+     594         468 :     diagnostics.current_values.kibxy_lim = it->second.kibxy_lim;
+     595             : 
+     596         468 :     diagnostics.current_values.kiwxy     = it->second.kiwxy;
+     597         468 :     diagnostics.current_values.kiwxy_lim = it->second.kiwxy_lim;
+     598             : 
+     599         468 :     diagnostics.current_values.kpz = it->second.kpz;
+     600         468 :     diagnostics.current_values.kvz = it->second.kvz;
+     601         468 :     diagnostics.current_values.kaz = it->second.kaz;
+     602             : 
+     603         468 :     diagnostics.current_values.kqy = it->second.kqy;
+     604             : 
+     605         468 :     diagnostics.current_values.km     = it->second.km;
+     606         468 :     diagnostics.current_values.km_lim = it->second.km_lim;
+     607             :   }
+     608             : 
+     609         468 :   ph_diagnostics_.publish(diagnostics);
+     610             : }
+     611             : 
+     612             : //}
+     613             : 
+     614             : // --------------------------------------------------------------
+     615             : // |                          routines                          |
+     616             : // --------------------------------------------------------------
+     617             : 
+     618             : /* stringInVector() //{ */
+     619             : 
+     620         924 : bool GainManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     621             : 
+     622         924 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     623          42 :     return false;
+     624             :   } else {
+     625         882 :     return true;
+     626             :   }
+     627             : }
+     628             : 
+     629             : //}
+     630             : 
+     631             : }  // namespace gain_manager
+     632             : 
+     633             : }  // namespace mrs_uav_managers
+     634             : 
+     635             : #include <pluginlib/class_list_macros.h>
+     636          42 : 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..99dde9c011 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html @@ -0,0 +1,179 @@ + + + + + + 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 + + +
+ 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..6ceeb4cc94ea7fff3233934c9b9015cf2640a8eb GIT binary patch literal 2109 zcmV-D2*US?P)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp8KD5nZ!^B+UfK=dl8xQFx63Mk7pEcF#5; z0<4&a#pC4X5sD{M91V*147;uaFX(zb4P$Myija4*w1M)%EY{6d%Yl7W0dD4rL(!Y3 z1zZ_|d(Py+KqhkZLx3OYJi~TysIfB$5&@ zR8!$Gr8vjnlBuUAZ?2yC$N?O8i_gJS(xZ@-CYKbKMB#x*o-}19tnvsccFTxROrqJA zGBTw{M*f(6!tf#$npc<@PY8p&3FD&|5%n6@(wrmAEuRUcLbKIRmIKHp!lq~Wwd?Bk zxx^^$yV^rjcd{+?zjB}>rgyD(tm#pTwn_46rKSn@=x zGljW(UvbYx0P2oCOWbAJ!0&SBq~*9n!emV;KobBbDdw#1(-eU>Ta{P425>zXaYgc; zDvv-xxtk&22!vX24FgUn9Gww6J=|>^HPn+%>H3URKs&mBV$Y?~j*^Lir&|nBv|P1- zqdXczt71z208onqJ+ucXqsNfhTmlrbwU@gMbgv}4R-R&;M>X5E_Zauf1I2R+AA8mi z!kMuru&L5FN2Rvv!{j3r?)ulk)RS(8^?d?$GOxI(7>%PmFv%djPKx#1@_4rwXvh0; zKEIxaf9w6~I1=Ea(oWz}0|cP_&N-P3M~gbAxY}_mJYMCv=HoTFc#f+(=fWZuW*t}{ z)5~5ESoa>Lmp!DtM_u7PS?JNhoS9Ag)55#SE*U`U3^gFPYx&L8L_OFhQvdAQqPX`` zls^zF+)?eso>bx`n#^<~MGuUBFx5i>wWCPxQ{-JYfsSHhPs$TTB5D9KTe>(a*JQvc z606jJ+^*$M?Zd7s?c8%b%I;`Z)Tp7tX~XClVWfgoqHPr!3*1ix(lwT|jPqD358GN? z^6A>n*w6RFz5%9T)^z;_@}}#a`8o*D%zKy$$4&u=CBO(p?NW4PJ7Qs`6dhe(a@bUD zTLdPbD~jU)kV&zVN6EVHy&N<4jchSB7gyoRyHKEBmsw3B*oCP~KfU>s!<58GLPS+pjkpYZ6veN7S zlgD~xLB=D+BhlIvR0Ypc0$*o~G=3!@DhzdjN64)s%0ut5EC@T)L}}mJFv=_CrO;YvfX9 zuOf~ptR3Os{4w*06pz|S{Nx_MBMMlhJtVb{P_q;t?ws@KERk%X4pPim^uOJK>L|tI zAb3N8ZHmvWa@$>cJ_uG%o;rizkb*Dg0 zSTqR}7U>M1V$!_l9*+JIfqfKzhY8KB>8h=(Q|==j|21KlJVsl;qG7F-$0`1z;mYMv zJ`b;sLcsY?8-=ZOMtT(f=Cwj`q4M$*NZVGCuU0`$Ai>FDT%-?OfvPU>UHLqR;&nA~ zaiNWFZ@{u0&np3BHl$1fKPvsOdSHQXUgZOMWOm(G9Kr}I*Btq#Oy-szxl8H`wyD}8 z?cEHN^HbGIy0->t4Uys&zMH{+F_U>~+fd6mK7ifTXvKNlfK=`e^XjZgh7<`FtnNBY zh?YN;1rVmVn69mht+QNNVt{CW5gRmk`i4w>5zuiqpWw!Y9NKy0f zzOIvbfgB1<%cQ)K)ubKwG;?Xe`G+5!sxy#^+b|!eab$;O-^)&M58g!t!y>bDX&k$e zJ_s)yo&LzKR%Ev;*9tOOIG%?qpckJc{^j8x$-@&D-FFvcReVeDpVi|??RD6DFb{Bm z;7c#RLmn=(`tTGhNqDFC + + + + + + 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:935169055.3 %
Date:2023-12-06 07:59:45Functions:547374.0 %
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 +
67.8%67.8%
+ 		67.8 %40 / 5955.0 %11 / 20
<unnamed>67.8 %40 / 5955.0 %11 / 20
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
<unnamed>65.2 %148 / 22775.0 %6 / 8
uav_manager.cpp +
47.4%47.4%
+ 		47.4 %545 / 115181.6 %31 / 38
<unnamed>47.4 %545 / 115181.6 %31 / 38
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
<unnamed>79.8 %202 / 25385.7 %6 / 7
+
+
+ + + + +
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..7b50ab2cae --- /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:935169055.3 %
Date:2023-12-06 07:59:45Functions:547374.0 %
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 +
47.4%47.4%
+ 		47.4 %545 / 115181.6 %31 / 38
<unnamed>47.4 %545 / 115181.6 %31 / 38
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
<unnamed>65.2 %148 / 22775.0 %6 / 8
null_tracker.cpp +
67.8%67.8%
+ 		67.8 %40 / 5955.0 %11 / 20
<unnamed>67.8 %40 / 5955.0 %11 / 20
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
<unnamed>79.8 %202 / 25385.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..566ab4efbf --- /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:935169055.3 %
Date:2023-12-06 07:59:45Functions:547374.0 %
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.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
<unnamed>65.2 %148 / 22775.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
<unnamed>79.8 %202 / 25385.7 %6 / 7
null_tracker.cpp +
67.8%67.8%
+ 		67.8 %40 / 5955.0 %11 / 20
<unnamed>67.8 %40 / 5955.0 %11 / 20
uav_manager.cpp +
47.4%47.4%
+ 		47.4 %545 / 115181.6 %31 / 38
<unnamed>47.4 %545 / 115181.6 %31 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-f.html b/mrs_uav_managers/src/index-sort-f.html new file mode 100644 index 0000000000..960b6a74cd --- /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:935169055.3 %
Date:2023-12-06 07:59:45Functions:547374.0 %
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 +
67.8%67.8%
+ 		67.8 %40 / 5955.0 %11 / 20
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
uav_manager.cpp +
47.4%47.4%
+ 		47.4 %545 / 115181.6 %31 / 38
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
+
+
+ + + + +
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..1dab703c6c --- /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:935169055.3 %
Date:2023-12-06 07:59:45Functions:547374.0 %
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 +
47.4%47.4%
+ 		47.4 %545 / 115181.6 %31 / 38
constraint_manager.cpp +
65.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
null_tracker.cpp +
67.8%67.8%
+ 		67.8 %40 / 5955.0 %11 / 20
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.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..9812c5bb3f --- /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:935169055.3 %
Date:2023-12-06 07:59:45Functions:547374.0 %
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.2%65.2%
+ 		65.2 %148 / 22775.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %202 / 25385.7 %6 / 7
null_tracker.cpp +
67.8%67.8%
+ 		67.8 %40 / 5955.0 %11 / 20
uav_manager.cpp +
47.4%47.4%
+ 		47.4 %545 / 115181.6 %31 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html b/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..6199a4ab6f --- /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:405967.8 %
Date:2023-12-06 07:59:45Functions: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&)1
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_managers::NullTracker::~NullTracker()42
mrs_uav_managers::NullTracker::~NullTracker().242
mrs_uav_managers::NullTracker::deactivate()87
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)90
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_managers::NullTracker::getStatus()1892
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
+
+
+ + + +
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..84574405a0 --- /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:405967.8 %
Date:2023-12-06 07:59:45Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::NullTracker::deactivate()87
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>)42
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&)123
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
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&)1
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&)39318
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)90
mrs_uav_managers::NullTracker::getStatus()1892
mrs_uav_managers::NullTracker::~NullTracker()42
mrs_uav_managers::NullTracker::~NullTracker().242
+
+
+ + + +
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..213c79b761 --- /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:405967.8 %
Date:2023-12-06 07:59:45Functions: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          84 :   ~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          42 : 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          84 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60          42 :   ros::Time::waitForValid();
+      61             : 
+      62          42 :   is_initialized = true;
+      63             : 
+      64          42 :   this->common_handlers = common_handlers;
+      65             : 
+      66          42 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68          84 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75          90 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77          90 :   std::stringstream ss;
+      78          90 :   ss << "activated";
+      79             : 
+      80          90 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81          90 :   is_active = true;
+      82             : 
+      83         180 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90          87 : void NullTracker::deactivate(void) {
+      91             : 
+      92          87 :   ROS_INFO("[NullTracker]: deactivated");
+      93          87 :   is_active = false;
+      94          87 : }
+      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       39318 : 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       39318 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126        1892 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128        1892 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130        1892 :   tracker_status.active            = is_active;
+     131        1892 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133        1892 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140           2 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142           4 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144           2 :   std::stringstream ss;
+     145             : 
+     146           2 :   if (cmd->data != callbacks_enabled) {
+     147             : 
+     148           2 :     callbacks_enabled = cmd->data;
+     149             : 
+     150           2 :     ss << "callbacks " << (callbacks_enabled ? "enabled" : "disabled");
+     151             : 
+     152           2 :     ROS_DEBUG_STREAM("[NullTracker]: " << ss.str());
+     153             : 
+     154             :   } else {
+     155             : 
+     156           0 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159           2 :   res.message = ss.str();
+     160           2 :   res.success = true;
+     161             : 
+     162           6 :   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           1 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr NullTracker::setTrajectoryReference([
+     187             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     188           1 :   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         123 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240         123 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248          42 : 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..734c215d319df4f4b445cf1fb1f75c33dea23bbc GIT binary patch literal 818 zcmV-21I_%2P)yYdI?mz#El8cxY5?wl@~C0+~i>&q9lR4}Ug+!GbPla>l?Ac~b32$w6AO)0LQc3vJ*#yG3aZZ$d zln)Qh8JS!I_A**q;~TWmxr(WSUfc2>(#P0v*aI8u4glHmjdJzHC01Js$2zZ-^UC{Y z;0RrPgM0(6Yh||wcB_V@v~(4Qloi`?=eX!APCY8kzS+Yi0?z9cpZ3&XWSO$q`BOcVzZd@=x|X#1YGb&g zrjl(9qzK3s-@_So{iq%;)hHA`2y`f@n~(MIz;}P!LqwKb0P7~24m=YYRT%C8-a~7 zw&M}98GrIk%ew9_Kc8RcoZdE=%G(C7YGux0zUnzTzFM9|zVU7(UZ#JSKI-t zbc(n+Jfu@Z@z&9!?z@k<#g=RC4dnJaZy@1tZfp3{H{yaV + + + + + + 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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
<unnamed>77.3 %337 / 43692.3 %12 / 13
+
+
+ + + + +
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..c34b80aebf --- /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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
<unnamed>77.3 %337 / 43692.3 %12 / 13
+
+
+ + + + +
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..348b9558ef --- /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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
<unnamed>77.3 %337 / 43692.3 %12 / 13
+
+
+ + + + +
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..d8c387f268 --- /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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
+
+
+ + + + +
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..74ffbcbd4c --- /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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
+
+
+ + + + +
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..6abb377099 --- /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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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 +
77.3%77.3%
+ 		77.3 %337 / 43692.3 %12 / 13
+
+
+ + + + +
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..0dec0b7fc1 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html @@ -0,0 +1,132 @@ + + + + + + + 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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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&) const3
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::transform_manager::TransformManager::onInit()42
mrs_uav_managers::transform_manager::TransformManager::TransformManager()42
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const632
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1279
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)31950
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)42247
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)46402
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)54544
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)138702
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)138702
+
+
+ + + +
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..c299eeb297 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html @@ -0,0 +1,132 @@ + + + + + + + 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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)31950
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1279
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)46402
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)42247
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)54544
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)138702
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)138702
mrs_uav_managers::transform_manager::TransformManager::onInit()42
mrs_uav_managers::transform_manager::TransformManager::TransformManager()42
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const632
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3
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..61f45111bd --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html @@ -0,0 +1,1030 @@ + + + + + + + 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:33743677.3 %
Date:2023-12-06 07:59:45Functions:121392.3 %
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          42 :   TransformManager() {
+      50          42 :     ch_ = std::make_shared<estimation_manager::CommonHandlers_t>();
+      51             : 
+      52          42 :     ch_->nodelet_name = nodelet_name_;
+      53          42 :     ch_->package_name = package_name_;
+      54          42 :   }
+      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_fixed_;
+      86             :   geometry_msgs::Pose pose_fixed_diff_;
+      87             : 
+      88             :   std::string ns_amsl_origin_parent_frame_id_;
+      89             :   std::string ns_amsl_origin_child_frame_id_;
+      90             :   bool        publish_amsl_origin_tf_;
+      91             : 
+      92             :   std::string          world_origin_units_;
+      93             :   geometry_msgs::Point world_origin_;
+      94             : 
+      95             :   std::vector<std::string>               tf_source_names_, estimator_names_;
+      96             :   std::vector<std::unique_ptr<TfSource>> tf_sources_;
+      97             : 
+      98             :   std::vector<std::string> utm_source_priority_list_;
+      99             :   std::string              utm_source_name_;
+     100             : 
+     101             :   std::mutex mtx_broadcast_utm_origin_;
+     102             :   std::mutex mtx_broadcast_world_origin_;
+     103             : 
+     104             :   ros::NodeHandle nh_;
+     105             : 
+     106             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     107             : 
+     108             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     109             : 
+     110             :   std::unique_ptr<TfMappingOrigin> tf_mapping_origin_;
+     111             : 
+     112             :   void timeoutCallback(const std::string& topic, const ros::Time& last_msg);
+     113             : 
+     114             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     115             :   void                                          callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     116             :   std::string                                   first_frame_id_;
+     117             :   std::string                                   last_frame_id_;
+     118             :   bool                                          is_first_frame_id_set_ = false;
+     119             : 
+     120             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_height_agl_;
+     121             :   void                                                callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg);
+     122             : 
+     123             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude> sh_altitude_amsl_;
+     124             :   void                                               callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg);
+     125             : 
+     126             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_hw_api_orientation_;
+     127             :   void                                                        callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg);
+     128             : 
+     129             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     130             :   void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+     131             :   std::atomic<bool>                                 got_utm_offset_ = false;
+     132             : 
+     133             :   mrs_lib::SubscribeHandler<mrs_msgs::RtkGps> sh_rtk_gps_;
+     134             :   void                                        callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg);
+     135             : 
+     136             :   std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;
+     137             : 
+     138             :   void publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg);
+     139             : };
+     140             : /*//}*/
+     141             : 
+     142             : 
+     143             : /*//{ onInit() */
+     144          42 : void TransformManager::onInit() {
+     145             : 
+     146          42 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     147             : 
+     148          42 :   ros::Time::waitForValid();
+     149             : 
+     150          42 :   ROS_INFO("[%s]: initializing", getPrintName().c_str());
+     151             : 
+     152          42 :   broadcaster_ = std::make_shared<mrs_lib::TransformBroadcaster>();
+     153             : 
+     154          42 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getPrintName());
+     155          42 :   ch_->transformer->retryLookupNewest(true);
+     156             : 
+     157          84 :   mrs_lib::ParamLoader param_loader(nh_, getPrintName());
+     158             : 
+     159          42 :   param_loader.loadParam("custom_config", _custom_config_);
+     160          42 :   param_loader.loadParam("platform_config", _platform_config_);
+     161          42 :   param_loader.loadParam("world_config", _world_config_);
+     162             : 
+     163          42 :   if (_custom_config_ != "") {
+     164          42 :     param_loader.addYamlFile(_custom_config_);
+     165             :   }
+     166             : 
+     167          42 :   if (_platform_config_ != "") {
+     168          42 :     param_loader.addYamlFile(_platform_config_);
+     169             :   }
+     170             : 
+     171          42 :   if (_world_config_ != "") {
+     172          42 :     param_loader.addYamlFile(_world_config_);
+     173             :   }
+     174             : 
+     175          42 :   param_loader.addYamlFileFromParam("private_config");
+     176          42 :   param_loader.addYamlFileFromParam("public_config");
+     177          42 :   param_loader.addYamlFileFromParam("estimators_config");
+     178             : 
+     179          84 :   const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+     180             : 
+     181          42 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     182             : 
+     183             :   /*//{ initialize scope timer */
+     184          42 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", ch_->scope_timer.enabled);
+     185          84 :   std::string       filepath;
+     186          84 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/transform_manager_scope_timer.txt";
+     187          42 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+     188             :   /*//}*/
+     189             : 
+     190             :   /*//{ load world_origin parameters */
+     191             : 
+     192          42 :   bool   is_origin_param_ok = true;
+     193          42 :   double world_origin_x     = 0;
+     194          42 :   double world_origin_y     = 0;
+     195             : 
+     196          42 :   param_loader.loadParam("world_origin/units", world_origin_units_);
+     197             : 
+     198          42 :   if (Support::toLowercase(world_origin_units_) == "utm") {
+     199           0 :     ROS_INFO("[%s]: Loading world origin in UTM units.", getPrintName().c_str());
+     200           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", world_origin_x);
+     201           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", world_origin_y);
+     202             : 
+     203          42 :   } else if (Support::toLowercase(world_origin_units_) == "latlon") {
+     204             :     double lat, lon;
+     205          42 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getPrintName().c_str());
+     206          42 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     207          42 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     208          42 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     209          42 :     ROS_INFO("[%s]: Converted to UTM x: %f, y: %f.", getPrintName().c_str(), world_origin_x, world_origin_y);
+     210             : 
+     211             :   } else {
+     212           0 :     ROS_ERROR("[%s]: world_origin_units must be (\"UTM\"|\"LATLON\"). Got '%s'", getPrintName().c_str(), world_origin_units_.c_str());
+     213           0 :     ros::shutdown();
+     214             :   }
+     215             : 
+     216          42 :   world_origin_.x = world_origin_x;
+     217          42 :   world_origin_.y = world_origin_y;
+     218          42 :   world_origin_.z = 0;
+     219             : 
+     220          42 :   if (!is_origin_param_ok) {
+     221           0 :     ROS_ERROR("[%s]: Could not load all mandatory parameters from world file. Please check your world file.", getPrintName().c_str());
+     222           0 :     ros::shutdown();
+     223             :   }
+     224             :   /*//}*/
+     225             : 
+     226             :   /*//{ load local_origin parameters */
+     227          84 :   std::string local_origin_parent_frame_id;
+     228          42 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/parent", local_origin_parent_frame_id);
+     229          42 :   ns_local_origin_parent_frame_id_ = ch_->uav_name + "/" + local_origin_parent_frame_id;
+     230             : 
+     231          84 :   std::string local_origin_child_frame_id;
+     232          42 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/child", local_origin_child_frame_id);
+     233          42 :   ns_local_origin_child_frame_id_ = ch_->uav_name + "/" + local_origin_child_frame_id;
+     234             : 
+     235          42 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/enabled", publish_local_origin_tf_);
+     236             :   /*//}*/
+     237             : 
+     238             :   /*//{ load stable_origin parameters */
+     239          84 :   std::string stable_origin_parent_frame_id;
+     240          42 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/parent", stable_origin_parent_frame_id);
+     241          42 :   ns_stable_origin_parent_frame_id_ = ch_->uav_name + "/" + stable_origin_parent_frame_id;
+     242             : 
+     243          84 :   std::string stable_origin_child_frame_id;
+     244          42 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/child", stable_origin_child_frame_id);
+     245          42 :   ns_stable_origin_child_frame_id_ = ch_->uav_name + "/" + stable_origin_child_frame_id;
+     246             : 
+     247          42 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/enabled", publish_stable_origin_tf_);
+     248             :   /*//}*/
+     249             : 
+     250             :   /*//{ load fixed_origin parameters */
+     251          84 :   std::string fixed_origin_parent_frame_id;
+     252          42 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/parent", fixed_origin_parent_frame_id);
+     253          42 :   ns_fixed_origin_parent_frame_id_ = ch_->uav_name + "/" + fixed_origin_parent_frame_id;
+     254             : 
+     255          84 :   std::string fixed_origin_child_frame_id;
+     256          42 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/child", fixed_origin_child_frame_id);
+     257          42 :   ns_fixed_origin_child_frame_id_ = ch_->uav_name + "/" + fixed_origin_child_frame_id;
+     258             : 
+     259          42 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/enabled", publish_fixed_origin_tf_);
+     260             :   /*//}*/
+     261             : 
+     262             :   /*//{ load fcu_untilted parameters */
+     263          84 :   std::string fcu_frame_id;
+     264          42 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/parent", fcu_frame_id);
+     265          42 :   ch_->frames.fcu    = fcu_frame_id;
+     266          42 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + fcu_frame_id;
+     267             : 
+     268          84 :   std::string fcu_untilted_frame_id;
+     269          42 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/child", fcu_untilted_frame_id);
+     270          42 :   ch_->frames.fcu_untilted    = fcu_untilted_frame_id;
+     271          42 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + fcu_untilted_frame_id;
+     272             : 
+     273          42 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_);
+     274             :   /*//}*/
+     275             : 
+     276             :   /*//{ load amsl_origin parameters*/
+     277          84 :   std::string amsl_parent_frame_id, amsl_child_frame_id;
+     278          42 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/enabled", publish_amsl_origin_tf_);
+     279          42 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/parent", amsl_parent_frame_id);
+     280          42 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/child", amsl_child_frame_id);
+     281          42 :   ch_->frames.amsl                = amsl_child_frame_id;
+     282          42 :   ch_->frames.ns_amsl             = ch_->uav_name + "/" + amsl_child_frame_id;
+     283          42 :   ns_amsl_origin_parent_frame_id_ = ch_->uav_name + "/" + amsl_parent_frame_id;
+     284          42 :   ns_amsl_origin_child_frame_id_  = ch_->uav_name + "/" + amsl_child_frame_id;
+     285             : 
+     286             :   /*//}*/
+     287             : 
+     288          42 :   param_loader.loadParam(yaml_prefix + "rtk_antenna/frame_id", ch_->frames.rtk_antenna);
+     289          42 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     290             : 
+     291          42 :   param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_);
+     292          42 :   param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_);
+     293             : 
+     294          42 :   param_loader.loadParam(yaml_prefix + "utm_source_priority", utm_source_priority_list_);
+     295         157 :   for (auto utm_source : utm_source_priority_list_) {
+     296         154 :     if (Support::isStringInVector(utm_source, estimator_names_)) {
+     297          39 :       ROS_INFO("[%s]: the source for utm_origin and world origin is: %s", getPrintName().c_str(), utm_source.c_str());
+     298          39 :       utm_source_name_ = utm_source;
+     299          39 :       break;
+     300             :     }
+     301             :   }
+     302             : 
+     303             :   /*//{ initialize tf sources */
+     304          42 :   for (size_t i = 0; i < tf_source_names_.size(); i++) {
+     305             : 
+     306           0 :     const std::string tf_source_name = tf_source_names_[i];
+     307           0 :     const bool        is_utm_source  = tf_source_name == utm_source_name_;
+     308             : 
+     309           0 :     ROS_INFO("[%s]: loading tf source: %s", getPrintName().c_str(), tf_source_name.c_str());
+     310             : 
+     311           0 :     auto source_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + tf_source_name);
+     312             : 
+     313           0 :     if (_custom_config_ != "") {
+     314           0 :       source_param_loader->addYamlFile(_custom_config_);
+     315             :     }
+     316             : 
+     317           0 :     if (_platform_config_ != "") {
+     318           0 :       source_param_loader->addYamlFile(_platform_config_);
+     319             :     }
+     320             : 
+     321           0 :     if (_world_config_ != "") {
+     322           0 :       source_param_loader->addYamlFile(_world_config_);
+     323             :     }
+     324             : 
+     325           0 :     source_param_loader->addYamlFileFromParam("private_config");
+     326           0 :     source_param_loader->addYamlFileFromParam("public_config");
+     327           0 :     source_param_loader->addYamlFileFromParam("estimators_config");
+     328             : 
+     329           0 :     tf_sources_.push_back(std::make_unique<TfSource>(tf_source_name, nh_, source_param_loader, broadcaster_, ch_, is_utm_source));
+     330             :   }
+     331             : 
+     332             :   // additionally publish tf of all available estimators
+     333          84 :   for (int i = 0; i < int(estimator_names_.size()); i++) {
+     334             : 
+     335          84 :     const std::string estimator_name = estimator_names_[i];
+     336          42 :     const bool        is_utm_source  = estimator_name == utm_source_name_;
+     337          42 :     ROS_INFO("[%s]: loading tf source of estimator: %s", getPrintName().c_str(), estimator_name.c_str());
+     338             : 
+     339          42 :     auto estimator_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + estimator_name);
+     340             : 
+     341          42 :     if (_custom_config_ != "") {
+     342          42 :       estimator_param_loader->addYamlFile(_custom_config_);
+     343             :     }
+     344             : 
+     345          42 :     if (_platform_config_ != "") {
+     346          42 :       estimator_param_loader->addYamlFile(_platform_config_);
+     347             :     }
+     348             : 
+     349          42 :     if (_world_config_ != "") {
+     350          42 :       estimator_param_loader->addYamlFile(_world_config_);
+     351             :     }
+     352             : 
+     353          42 :     estimator_param_loader->addYamlFileFromParam("private_config");
+     354          42 :     estimator_param_loader->addYamlFileFromParam("public_config");
+     355          42 :     estimator_param_loader->addYamlFileFromParam("estimators_config");
+     356             : 
+     357          42 :     tf_sources_.push_back(std::make_unique<TfSource>(estimator_name, nh_, estimator_param_loader, broadcaster_, ch_, is_utm_source));
+     358             :   }
+     359             : 
+     360             :   // initialize mapping_origin tf
+     361             :   bool mapping_origin_tf_enabled;
+     362          42 :   param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled, false);
+     363             : 
+     364          42 :   if (mapping_origin_tf_enabled) {
+     365             : 
+     366           0 :     auto mapping_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/mapping_origin_tf");
+     367             : 
+     368           0 :     if (_custom_config_ != "") {
+     369           0 :       mapping_param_loader->addYamlFile(_custom_config_);
+     370             :     }
+     371             : 
+     372           0 :     if (_platform_config_ != "") {
+     373           0 :       mapping_param_loader->addYamlFile(_platform_config_);
+     374             :     }
+     375             : 
+     376           0 :     if (_world_config_ != "") {
+     377           0 :       mapping_param_loader->addYamlFile(_world_config_);
+     378             :     }
+     379             : 
+     380           0 :     mapping_param_loader->addYamlFileFromParam("private_config");
+     381           0 :     mapping_param_loader->addYamlFileFromParam("public_config");
+     382           0 :     mapping_param_loader->addYamlFileFromParam("estimators_config");
+     383             : 
+     384           0 :     tf_mapping_origin_ = std::make_unique<TfMappingOrigin>(nh_, mapping_param_loader, broadcaster_, ch_);
+     385             :   }
+     386             : 
+     387             :   //}
+     388             : 
+     389             :   /*//{ initialize subscribers */
+     390          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+     391          42 :   shopts.nh                 = nh_;
+     392          42 :   shopts.node_name          = getPrintName();
+     393          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     394          42 :   shopts.threadsafe         = true;
+     395          42 :   shopts.autostart          = true;
+     396          42 :   shopts.queue_size         = 10;
+     397          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     398             : 
+     399          42 :   sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &TransformManager::callbackUavState, this);
+     400             : 
+     401          42 :   sh_height_agl_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this);
+     402             : 
+     403          42 :   sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this);
+     404             : 
+     405             :   sh_hw_api_orientation_ =
+     406          42 :       mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this);
+     407             : 
+     408          42 :   if (utm_source_name_ == "rtk" || utm_source_name_ == "rtk_garmin") {
+     409           2 :     sh_rtk_gps_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, "rtk_gps_in", &TransformManager::callbackRtkGps, this);
+     410             :   } else {
+     411          40 :     sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &TransformManager::callbackGnss, this);
+     412             :   }
+     413             :   /*//}*/
+     414             : 
+     415          42 :   if (!param_loader.loadedSuccessfully()) {
+     416           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     417           0 :     ros::shutdown();
+     418             :   }
+     419             : 
+     420          42 :   is_initialized_ = true;
+     421          42 :   ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     422          42 : }
+     423             : /*//}*/
+     424             : 
+     425             : /*//{ callbackUavState() */
+     426             : 
+     427       46402 : void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+     428             : 
+     429       46402 :   if (!is_initialized_) {
+     430        1297 :     return;
+     431             :   }
+     432             : 
+     433       92804 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     434             : 
+     435             :   // obtain first frame_id
+     436       46402 :   if (!is_first_frame_id_set_) {
+     437          42 :     first_frame_id_                = msg->header.frame_id;
+     438          42 :     last_frame_id_                 = msg->header.frame_id;
+     439          42 :     pose_fixed_                    = msg->pose;
+     440          42 :     pose_fixed_diff_.orientation.w = 1;
+     441          42 :     is_first_frame_id_set_         = true;
+     442             :   }
+     443             : 
+     444       46402 :   if (publish_local_origin_tf_) {
+     445             :     /*//{ publish local_origin tf*/
+     446       46402 :     geometry_msgs::TransformStamped tf_msg;
+     447       46402 :     tf_msg.header.stamp    = msg->header.stamp;
+     448       46402 :     tf_msg.header.frame_id = ns_local_origin_parent_frame_id_;
+     449       46402 :     tf_msg.child_frame_id  = ns_local_origin_child_frame_id_;
+     450             : 
+     451             :     // transform pose to first frame_id
+     452       46402 :     geometry_msgs::PoseStamped pose;
+     453       46402 :     pose.header = msg->header;
+     454       46402 :     pose.pose   = msg->pose;
+     455             : 
+     456       46402 :     if (pose.pose.orientation.w == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.x == 0) {
+     457           1 :       ROS_WARN_ONCE("[%s]: Uninitialized quaternion detected during publishing stable_origin tf of %s. Setting w=1", getPrintName().c_str(),
+     458             :                     pose.header.frame_id.c_str());
+     459           1 :       pose.pose.orientation.w = 1.0;
+     460             :     }
+     461             : 
+     462       92804 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_.substr(0, first_frame_id_.find("_origin")) + "_local_origin");
+     463             : 
+     464       46402 :     if (res) {
+     465       45105 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     466       45105 :       const tf2::Transform      tf_inv   = tf.inverse();
+     467       45105 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     468       45105 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     469       45105 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     470             : 
+     471       45105 :       if (Support::noNans(tf_msg)) {
+     472             :         try {
+     473       45105 :           broadcaster_->sendTransform(tf_msg);
+     474             :         }
+     475           0 :         catch (...) {
+     476           0 :           ROS_ERROR("exception caught ");
+     477             :         }
+     478             :       } else {
+     479           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(),
+     480             :                           tf_msg.child_frame_id.c_str());
+     481             :       }
+     482       45105 :       ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     483             :                     tf_msg.child_frame_id.c_str());
+     484             :     } else {
+     485        1297 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not publishing local_origin transform.", getPrintName().c_str(), first_frame_id_.c_str());
+     486        1297 :       return;
+     487             :     }
+     488             :     /*//}*/
+     489             :   }
+     490             : 
+     491       45105 :   if (publish_stable_origin_tf_) {
+     492             :     /*//{ publish stable_origin tf*/
+     493       45105 :     geometry_msgs::TransformStamped tf_msg;
+     494       45105 :     tf_msg.header.stamp    = msg->header.stamp;
+     495       45105 :     tf_msg.header.frame_id = ns_stable_origin_parent_frame_id_;
+     496       45105 :     tf_msg.child_frame_id  = ns_stable_origin_child_frame_id_;
+     497             : 
+     498             :     // transform pose to first frame_id
+     499       45105 :     geometry_msgs::PoseStamped pose;
+     500       45105 :     pose.header = msg->header;
+     501       45105 :     pose.pose   = msg->pose;
+     502       45105 :     if (pose.pose.orientation.w == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.x == 0) {
+     503           0 :       ROS_WARN_ONCE("[%s]: Uninitialized quaternion detected during publishing stable_origin tf of %s. Setting w=1", getPrintName().c_str(),
+     504             :                     pose.header.frame_id.c_str());
+     505           0 :       pose.pose.orientation.w = 1.0;
+     506             :     }
+     507             : 
+     508       45105 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_);
+     509             : 
+     510       45105 :     if (res) {
+     511       45105 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     512       45105 :       const tf2::Transform      tf_inv   = tf.inverse();
+     513       45105 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     514       45105 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     515       45105 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     516             : 
+     517       45105 :       if (Support::noNans(tf_msg)) {
+     518             :         try {
+     519       45105 :           broadcaster_->sendTransform(tf_msg);
+     520             :         }
+     521           0 :         catch (...) {
+     522           0 :           ROS_ERROR("exception caught ");
+     523             :         }
+     524             :       } else {
+     525           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(),
+     526             :                           tf_msg.child_frame_id.c_str());
+     527             :       }
+     528       45105 :       ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     529             :                     tf_msg.child_frame_id.c_str());
+     530             :     } else {
+     531           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not publishing stable_origin transform.", getPrintName().c_str(), first_frame_id_.c_str());
+     532           0 :       return;
+     533             :     }
+     534             :     /*//}*/
+     535             :   }
+     536             : 
+     537       45105 :   if (publish_fixed_origin_tf_) {
+     538             :     /*//{ publish fixed_origin tf*/
+     539       45105 :     if (msg->header.frame_id != last_frame_id_) {
+     540           0 :       ROS_WARN("[%s]: Detected estimator change from %s to %s. Updating offset for fixed origin.", getPrintName().c_str(), last_frame_id_.c_str(),
+     541             :                msg->header.frame_id.c_str());
+     542             : 
+     543           0 :       pose_fixed_diff_ = Support::getPoseDiff(msg->pose, pose_fixed_);
+     544             :     }
+     545             : 
+     546             : 
+     547       45105 :     pose_fixed_ = Support::applyPoseDiff(msg->pose, pose_fixed_diff_);
+     548             : 
+     549       90210 :     geometry_msgs::TransformStamped tf_msg;
+     550       45105 :     tf_msg.header.stamp    = msg->header.stamp;
+     551       45105 :     tf_msg.header.frame_id = ns_fixed_origin_parent_frame_id_;
+     552       45105 :     tf_msg.child_frame_id  = ns_fixed_origin_child_frame_id_;
+     553             : 
+     554       45105 :     const tf2::Transform      tf       = Support::tf2FromPose(pose_fixed_);
+     555       45105 :     const tf2::Transform      tf_inv   = tf.inverse();
+     556       45105 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     557       45105 :     tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     558       45105 :     tf_msg.transform.rotation          = pose_inv.orientation;
+     559             : 
+     560       45105 :     if (Support::noNans(tf_msg)) {
+     561             :       try {
+     562       45105 :         broadcaster_->sendTransform(tf_msg);
+     563             :       }
+     564           0 :       catch (...) {
+     565           0 :         ROS_ERROR("exception caught ");
+     566             :       }
+     567             :     } else {
+     568           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(),
+     569             :                         tf_msg.child_frame_id.c_str());
+     570             :     }
+     571       45105 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     572             :                   tf_msg.child_frame_id.c_str());
+     573             :     /*//}*/
+     574             :   }
+     575             : 
+     576             :   /*//{ choose another source of utm and world tfs after estimator switch */
+     577       45105 :   if (msg->header.frame_id != last_frame_id_) {
+     578           0 :     const std::string last_estimator_name    = Support::frameIdToEstimatorName(last_frame_id_);
+     579           0 :     const std::string current_estimator_name = Support::frameIdToEstimatorName(msg->header.frame_id);
+     580             : 
+     581           0 :     ROS_INFO("[%s]: Detected estimator switch: %s -> %s", getPrintName().c_str(), last_estimator_name.c_str(), current_estimator_name.c_str());
+     582             : 
+     583           0 :     bool   valid_utm_source_found = false;
+     584             :     size_t potential_utm_source_index;
+     585             : 
+     586           0 :     for (size_t i = 0; i < tf_sources_.size(); i++) {
+     587             : 
+     588             :       // first check if tf source can publish utm origin and is not the switched-from estimator
+     589           0 :       if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() != last_estimator_name) {
+     590             : 
+     591           0 :         valid_utm_source_found     = true;
+     592           0 :         potential_utm_source_index = i;
+     593             : 
+     594             :         // check if switched-to estimator is utm_based, if so, use it
+     595           0 :         if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() == current_estimator_name) {
+     596           0 :           potential_utm_source_index = i;
+     597           0 :           break;
+     598             :         }
+     599             :       }
+     600             :     }
+     601             : 
+     602             : 
+     603             :     // if we found a valid utm source, use it, otherwise stay with the switched-from estimator
+     604           0 :     if (valid_utm_source_found) {
+     605             : 
+     606             :       // stop all estimators from publishing utm source
+     607           0 :       for (size_t i = 0; i < tf_sources_.size(); i++) {
+     608           0 :         if (tf_sources_.at(i)->getIsUtmSource()) {
+     609           0 :           tf_sources_.at(i)->setIsUtmSource(false);
+     610           0 :           ROS_INFO("[%s]: setting is_utm_source of estimator %s to false", getPrintName().c_str(), last_estimator_name.c_str());
+     611             :         }
+     612             :       }
+     613             : 
+     614           0 :       tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
+     615           0 :       ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
+     616             :     }
+     617             :   }
+     618             :   /*//}*/
+     619             : 
+     620       45105 :   last_frame_id_ = msg->header.frame_id;
+     621             : }
+     622             : /*//}*/
+     623             : 
+     624             : /*//{ callbackHeightAgl() */
+     625             : 
+     626       42247 : void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg) {
+     627             : 
+     628       42247 :   if (!is_initialized_) {
+     629           0 :     return;
+     630             :   }
+     631             : 
+     632      126741 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackHeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     633             : 
+     634       84494 :   geometry_msgs::TransformStamped tf_msg;
+     635       42247 :   tf_msg.header.stamp    = msg->header.stamp;
+     636       42247 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     637       42247 :   tf_msg.child_frame_id  = msg->header.frame_id;
+     638             : 
+     639       42247 :   tf_msg.transform.translation.x = 0;
+     640       42247 :   tf_msg.transform.translation.y = 0;
+     641       42247 :   tf_msg.transform.translation.z = -msg->value;
+     642       42247 :   tf_msg.transform.rotation.x    = 0;
+     643       42247 :   tf_msg.transform.rotation.y    = 0;
+     644       42247 :   tf_msg.transform.rotation.z    = 0;
+     645       42247 :   tf_msg.transform.rotation.w    = 1;
+     646             : 
+     647       42247 :   if (Support::noNans(tf_msg)) {
+     648             :     try {
+     649       42247 :       broadcaster_->sendTransform(tf_msg);
+     650             :     }
+     651           0 :     catch (...) {
+     652           0 :       ROS_ERROR("exception caught ");
+     653             :     }
+     654             :   } else {
+     655           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(),
+     656             :                       tf_msg.child_frame_id.c_str());
+     657             :   }
+     658       42247 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     659             :                 tf_msg.child_frame_id.c_str());
+     660             : }
+     661             : /*//}*/
+     662             : 
+     663             : /*//{ callbackAltitudeAmsl() */
+     664             : 
+     665       54544 : void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg) {
+     666             : 
+     667       54544 :   if (!is_initialized_) {
+     668           0 :     return;
+     669             :   }
+     670             : 
+     671      163632 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackAltitudeAmsl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     672             : 
+     673      109088 :   geometry_msgs::TransformStamped tf_msg;
+     674       54544 :   tf_msg.header.stamp    = msg->stamp;
+     675       54544 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     676       54544 :   tf_msg.child_frame_id  = ch_->frames.ns_amsl;
+     677             : 
+     678       54544 :   tf_msg.transform.translation.x = 0;
+     679       54544 :   tf_msg.transform.translation.y = 0;
+     680       54544 :   tf_msg.transform.translation.z = -msg->amsl;
+     681       54544 :   tf_msg.transform.rotation.x    = 0;
+     682       54544 :   tf_msg.transform.rotation.y    = 0;
+     683       54544 :   tf_msg.transform.rotation.z    = 0;
+     684       54544 :   tf_msg.transform.rotation.w    = 1;
+     685             : 
+     686       54544 :   if (Support::noNans(tf_msg)) {
+     687             :     try {
+     688       54537 :       broadcaster_->sendTransform(tf_msg);
+     689             :     }
+     690           0 :     catch (...) {
+     691           0 :       ROS_ERROR("exception caught ");
+     692             :     }
+     693             :   } else {
+     694           7 :     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(),
+     695             :                       tf_msg.child_frame_id.c_str());
+     696             :   }
+     697       54544 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     698             :                 tf_msg.child_frame_id.c_str());
+     699             : }
+     700             : /*//}*/
+     701             : 
+     702             : /*//{ callbackHwApiOrientation() */
+     703      138702 : void TransformManager::callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     704             : 
+     705      138702 :   if (!is_initialized_) {
+     706           0 :     return;
+     707             :   }
+     708             : 
+     709      416106 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     710             : 
+     711      138702 :   if (publish_fcu_untilted_tf_) {
+     712      138702 :     publishFcuUntiltedTf(msg);
+     713             :   }
+     714             : }
+     715             : /*//}*/
+     716             : 
+     717             : /*//{ callbackGnss() */
+     718       31950 : void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+     719             : 
+     720       31950 :   if (!is_initialized_) {
+     721       31910 :     return;
+     722             :   }
+     723             : 
+     724       31950 :   if (got_utm_offset_) {
+     725       31910 :     return;
+     726             :   }
+     727             : 
+     728          80 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackGnss", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     729             : 
+     730             :   double out_x;
+     731             :   double out_y;
+     732             : 
+     733          40 :   mrs_lib::UTM(msg->latitude, msg->longitude, &out_x, &out_y);
+     734             : 
+     735          40 :   if (!std::isfinite(out_x)) {
+     736           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     737           0 :     return;
+     738             :   }
+     739             : 
+     740          40 :   if (!std::isfinite(out_y)) {
+     741           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     742           0 :     return;
+     743             :   }
+     744             : 
+     745          40 :   geometry_msgs::Point utm_origin;
+     746          40 :   utm_origin.x = out_x;
+     747          40 :   utm_origin.y = out_y;
+     748          40 :   utm_origin.z = msg->altitude;
+     749             : 
+     750          40 :   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);
+     751             : 
+     752          80 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     753          40 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     754          40 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     755             :   }
+     756          40 :   got_utm_offset_ = true;
+     757             : }
+     758             : /*//}*/
+     759             : 
+     760             : /*//{ callbackRtkGps() */
+     761        1279 : void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) {
+     762             : 
+     763        1279 :   if (!is_initialized_) {
+     764        1277 :     return;
+     765             :   }
+     766             : 
+     767        1279 :   if (got_utm_offset_) {
+     768        1276 :     return;
+     769             :   }
+     770             : 
+     771           6 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackRtkGps", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     772             : 
+     773             :   double out_x;
+     774             :   double out_y;
+     775             : 
+     776           3 :   geometry_msgs::PoseStamped rtk_pos;
+     777             : 
+     778           3 :   if (!std::isfinite(msg->gps.latitude)) {
+     779           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+     780           0 :     return;
+     781             :   }
+     782             : 
+     783           3 :   if (!std::isfinite(msg->gps.longitude)) {
+     784           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+     785           0 :     return;
+     786             :   }
+     787             : 
+     788           3 :   if (!std::isfinite(msg->gps.altitude)) {
+     789           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+     790           0 :     return;
+     791             :   }
+     792             : 
+     793           3 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+     794           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     795           0 :     return;
+     796             :   }
+     797             : 
+     798           3 :   rtk_pos.header = msg->header;
+     799           3 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+     800           3 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+     801           3 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     802             : 
+     803           3 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+     804           3 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+     805             : 
+     806             : 
+     807             :   // transform the RTK position from antenna to FCU
+     808           3 :   auto res = transformRtkToFcu(rtk_pos);
+     809           3 :   if (res) {
+     810           2 :     rtk_pos.pose = res.value();
+     811             :   } else {
+     812           1 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+     813           1 :     return;
+     814             :   }
+     815             : 
+     816           2 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &out_x, &out_y);
+     817             : 
+     818           2 :   if (!std::isfinite(out_x)) {
+     819           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     820           0 :     return;
+     821             :   }
+     822             : 
+     823           2 :   if (!std::isfinite(out_y)) {
+     824           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     825           0 :     return;
+     826             :   }
+     827             : 
+     828           2 :   geometry_msgs::Point utm_origin;
+     829           2 :   utm_origin.x = out_x;
+     830           2 :   utm_origin.y = out_y;
+     831           2 :   utm_origin.z = msg->gps.altitude;
+     832             : 
+     833           2 :   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);
+     834             : 
+     835           4 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     836           2 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     837           2 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     838             :   }
+     839           2 :   got_utm_offset_ = true;
+     840             : }
+     841             : /*//}*/
+     842             : 
+     843             : /*//{ publishFcuUntiltedTf() */
+     844      138702 : void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     845             : 
+     846      277404 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     847             : 
+     848             :   double heading;
+     849             : 
+     850             :   try {
+     851      138702 :     heading = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     852             :   }
+     853           0 :   catch (...) {
+     854           0 :     ROS_ERROR("[%s]: Exception caught during getting heading", getPrintName().c_str());
+     855           0 :     return;
+     856             :   }
+     857      138702 :   scope_timer.checkpoint("heading");
+     858             : 
+     859      138702 :   const Eigen::Matrix3d odom_pixhawk_R = mrs_lib::AttitudeConverter(msg->quaternion);
+     860      138702 :   const Eigen::Matrix3d undo_heading_R = mrs_lib::AttitudeConverter(Eigen::AngleAxis(-heading, Eigen::Vector3d(0, 0, 1)));
+     861             : 
+     862      138702 :   const tf2::Quaternion q     = mrs_lib::AttitudeConverter(undo_heading_R * odom_pixhawk_R);
+     863      138702 :   const tf2::Quaternion q_inv = q.inverse();
+     864             : 
+     865      138702 :   scope_timer.checkpoint("q inverse");
+     866             : 
+     867      138702 :   geometry_msgs::TransformStamped tf;
+     868      138702 :   tf.header.stamp            = msg->header.stamp;  // TODO(petrlmat) ros::Time::now()?
+     869      138702 :   tf.header.frame_id         = ch_->frames.ns_fcu;
+     870      138702 :   tf.child_frame_id          = ch_->frames.ns_fcu_untilted;
+     871      138702 :   tf.transform.translation.x = 0.0;
+     872      138702 :   tf.transform.translation.y = 0.0;
+     873      138702 :   tf.transform.translation.z = 0.0;
+     874      138702 :   tf.transform.rotation      = mrs_lib::AttitudeConverter(q_inv);
+     875             : 
+     876      138702 :   scope_timer.checkpoint("tf fill");
+     877      138702 :   if (Support::noNans(tf)) {
+     878      138702 :     broadcaster_->sendTransform(tf);
+     879             :   } else {
+     880           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN encountered in fcu_untilted tf", getPrintName().c_str());
+     881             :   }
+     882      138702 :   scope_timer.checkpoint("tf pub");
+     883             : }
+     884             : /*//}*/
+     885             : 
+     886             : /*//{ transformRtkToFcu() */
+     887           3 : std::optional<geometry_msgs::Pose> TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+     888             : 
+     889           6 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+     890             : 
+     891             :   // inject current orientation into rtk pose
+     892           6 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+     893           3 :   if (res1) {
+     894           2 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+     895             :   } else {
+     896           1 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s.", getPrintName().c_str(),
+     897             :                        ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str());
+     898           1 :     return {};
+     899             :   }
+     900             : 
+     901             :   // invert tf
+     902           2 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+     903           4 :   geometry_msgs::PoseStamped utm_in_antenna;
+     904           2 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+     905           2 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+     906           2 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+     907             : 
+     908             :   // transform to fcu
+     909           4 :   geometry_msgs::PoseStamped utm_in_fcu;
+     910           2 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+     911           2 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+     912           4 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+     913             : 
+     914           2 :   if (res2) {
+     915           2 :     utm_in_fcu = res2.value();
+     916             :   } else {
+     917           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(), ch_->frames.ns_fcu.c_str());
+     918           0 :     return {};
+     919             :   }
+     920             : 
+     921             :   // invert tf
+     922           2 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+     923           2 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+     924             : 
+     925           2 :   return fcu_in_utm;
+     926             : }
+     927             : /*//}*/
+     928             : 
+     929             : /*//{ getName() */
+     930           0 : std::string TransformManager::getName() const {
+     931           0 :   return name_;
+     932             : }
+     933             : /*//}*/
+     934             : 
+     935             : /*//{ getPrintName() */
+     936         632 : std::string TransformManager::getPrintName() const {
+     937         632 :   return nodelet_name_;
+     938             : }
+     939             : /*//}*/
+     940             : 
+     941             : }  // namespace transform_manager
+     942             : 
+     943             : }  // namespace mrs_uav_managers
+     944             : 
+     945             : #include <pluginlib/class_list_macros.h>
+     946          42 : 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..83f104c1f8 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html @@ -0,0 +1,257 @@ + + + + + + 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 + + +
+ 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..42ee6bc4c409e50fa4f215e5cf32bc003df85bf8 GIT binary patch literal 3294 zcmV<43?cK0P)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vprBhU@tHcfDS& z!%^i$&6B*gNEV<0+*x%f7b{S#e-DtaodV6wi0B$9(li!A(F~Na6;SFL4BX2I-$t_> zg{HDVQP(!LaSt($;%<6{Vv|ubu)Aumn1Kk)0lTW+#msS%NfcibxS?SO*GsF`$e&U5 z1=Kyt6+qMoU+k>=ddZ~r>V#b>ZgVQrO)JCgHaj;f#buzatQ6B%6XCr;XRg`^9O7uT zNPL7!v7_$_#WWZ$`;>v97-pf}t+Z3EEEI5tL72cwDb^UY1DNeQgOH&L55mv)^ZaIeS{m17JhpvaW1l?dd< z?UENr?dV6;En{MLeHznqCvj1Rl$7E*!S-OqWq{-z5qN>`G^_4hHVJ)QuUuqlqAwj$ zb?(_qqiR!xrK@VldFVnDnFFf6P#)CSQjt@6&Bkr6arxKLLZOYE?Un;epoROyGqTTJ z6tS8JybWfWoT&e(#O5o`e=IL|8Tr1FW!4z zST*w0Hcn2SP{cOYUpS?WVj)$FIA;a!7&9*RwEv%*Rl6yYG66bU+L!}FVLdI|8>VV7 ztrgZHaFtjsWKBF3%|seio#=6Mss~y$&8}uaJR?|7v@LQ|ac^Y`aAJc!GgD^TiDt6{ zXe`oD03z?F$lAbl3Pdx^aKAr8d4!+!*Yfbcp3Ae80`#Br<B0r{ z80^gl?dwJt-XF%k?itrXI<68O9SZ%xmi8D0jz*!f%Z3HMy#B=z_=xX4;@CVx;Kw^% z);au!bzkhAhAq@q_6hg;4ZqSmHIp{VK(URPLIq5E=^S3M>{_K@9~D-8p1n@-L3>}b zK35Mj7D4Xp>yOk&x$0E+JJ)9aj~svF`p~{s+FSGxORtF{(r_5-vKLBI0T#zFxU5rtkzTtw z*cs9|i5U;HaBYk$o|dHP?U2044EpGO`q%iF=$wo^FneFGKG)+}+_9HKRw5(7%}T)a zIewUckK#xNpwT%a(U~PVgD8;F6-!wumewyY#8b-BNik7bTev8|J6jv|W?w*?^J-Tf z&_=b9Y8#&zcw!sZ0Gvo(YvY3ePiUiBK-DVlVFhOBhA|_SB?hk!k4X(+#e7+3s!Lfv zBb=Y4Uh|WqdlY1r>PxA{(S0!cxf-}W#}5@MZ-@=$3;GQb}vG!#8LL zn4Z@-#bV3uYWiGp{BGZs(uRVKB3{ls`2D8GpD$+~82FjX8N77;#O2H%EDP0=>N{-R zlcAfIGu&!DYy2t8nZw;Uko}-eQgy(0R>HG+GL?6JNf<}o3&d4BA=ra}E>di5Eas}4 zz(`TfEu3Mq>UEUMKx0W1wS#{Cqy7+yoZ%Nf-Vk)rk0bW%h{F_EO>b|8wSw!a8rZ_g z!Nqrenj-Ml;@L&6X^2Ct+Edi~WtzKIDJO&#nam_^4@ufOPwRr^S>>g)X53SA6*Q63 zG+Wf=2x#n>+c4W=`yC5SnCQJXIV+Tzi+s|(xyk3w7`cWnmo9MQ=EtcSl}fte+&7zz zb;VTf0)z#C8D&CWI=KJqf+d{eOlnm>1T%T)?w-Y9m=a*mO zr2^b$a`7qd27#)jJ}GU4HHnMdlY%Z2zf?f)^~$lmP6=FtTOZaRPAzNTn(@n|4q#yF z4??j3$-8_4GzNOGk@SascY9%#w~SQkGbAyHo`1t+)os*BGXm4KF2 z*Q%1XBvxE=tlB@N#LC4-POu|RooiaPam-W`KuqCHqkr;ie0c((2fxgjyO>l=XpS4i zQ;0stmA!G{(=(-5eSpJ*3LRlZWQMhgl=cWOt;5W)V}Z39M)ulPhX2D18?7syQ1Y*I zVj-qX58|L^<{miYACiA~Lj2KUUHkg_XI-0I|E#N~xIgQvat)jR%ps>|$SGLjiwJCs zyk|=J6zh77lMBzqIEgY4csY-@0QE|}kkpLqkJPfYa}D~s+ICrhAvwV+*n38b43{^` z++!)<6@IW-xM<=`77hay3s=jLt$N1i3unc`ZI4sv=_UpX7j0oxc%UuTe79M+JFzpO zh4VnuFdQ~c_M(NunFWf4^Hq0^>(G5g)u@F75hT(#)kOM6^D*S>qe66g4EKbO`9i5N zrt=z&0+)}wO1j=0XMx-<`o?_RY>$E}JT_B=cFU)*h*jgJ^AyeHMbW1fzp7k-P!>8Y z?aU0p)%5oognkW~kU;@_K4jRNxWf#VQuBkp?vW!JeRr=92H>g>;*?_MdWbyd46IAa z#`5ZZNdmXAO;`3}BQvns#`Qn53t-frcU{#qtl|!#j5Lv?StSHH((^#XEt;EN4Zq|p zmh`*28}4o#U6N8%IbDoWB(@Y_ywxE*E>YXUF@onQN?|MlD-~PB1>jf-?{%!h&8(bu zR!Z0$Ehf&qy?@f-{gfJd$W@yB_I~~53ETTdKm5#`kK?xYjlLN^h^Y8xm?F*Xm4HrD z-tEi?yZgIZ-K_rmC9l9_JkCF9%$SeGJ1O?6fb*wJwccLD{r2fw}&E{ zsvWl58KbvXW(v|bwT8dX!Xfi*T*7gaD4{37*3@)OJ%+vY;0iBu&h@?goWGa^(YzqQ{ z%d6`2Cp*wuwY{XYgpiXEuSH!`Vd|c#jsgf=n=<^%x~JXOeHRA+J(yD8%`;p^e1_31 z%`8vFo%*>_-@r`aL>w@;zi_2Q_|KIxa{Yg}Qo1*F##SY!ahMBmq>Qc?>(L#B1E6zu z?J@2Q!CSPzA(E5`ckV*79x_s2Te}dPLSQcYgi2usZmHUFH&4YLCRNKPMK^BwDhEpU z?1(q^UBky<&d=V!lm$K)PJyh&s;eU(GnxnMThp6&hn)d*!qJ%sG!`ZXAOhi80F7cb zx+I`=jVr8sN}6`e^xrxG$SaFaF>z|vCl$Ap+qp+69NzTEKr6*`57IR!t+(z`>igW?)@wujajZ8`{z{^5)&vJ-=CC^ax;m+BRPx~}VVw&(% zO+t+JaJoaXnmkP*e}GA{sk@I*WE%&&&??6mEU2Rcf_Na;EX8WF=QyE2ndblmCPThN zu+zWp7+m_F1-k8=?8|}fCFgxU)9I@NmxZT}nb?oq%*Hqs!aA;V4p%+!1bn}VYfBP@ zYJqChPR0;V&poK^1>nHDaHM5jAvg_VS&;N ci;J880bugt>SLox4gdfE07*qoM6N<$f`|oFV*mgE literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..bcf7acf6fe --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:545115147.4 %
Date:2023-12-06 07:59:45Functions:313881.6 %
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::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::ungripSrv()0
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::landWithDescendImpl[abi:cxx11]()2
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)2
mrs_uav_managers::uav_manager::UavManager::landSrv()2
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()2
mrs_uav_managers::uav_manager::UavManager::disarmSrv()2
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)4
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()6
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)6
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)33
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)33
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()33
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)33
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)33
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::uav_manager::UavManager::initialize()42
mrs_uav_managers::uav_manager::UavManager::preinitialize()42
mrs_uav_managers::uav_manager::UavManager::onInit()42
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)46
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)46
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)47
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)80
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)177
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)287
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)623
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)6413
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)6413
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)14795
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)31133
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)46391
+
+
+ + + +
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..d39e886e8e --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.func.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:545115147.4 %
Date:2023-12-06 07:59:45Functions:313881.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_managers::uav_manager::UavManager::initialize()42
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()6
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)33
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&)177
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)287
mrs_uav_managers::uav_manager::UavManager::preinitialize()42
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)6413
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)6413
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)6
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)46
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)46391
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)623
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)14795
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)31133
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)4
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()2
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)80
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)33
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()33
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)33
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)2
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)46
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)47
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)33
mrs_uav_managers::uav_manager::UavManager::onInit()42
mrs_uav_managers::uav_manager::UavManager::landSrv()2
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()2
mrs_uav_managers::uav_manager::UavManager::disarmSrv()2
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::ungripSrv()0
+
+
+ + + +
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..9688ac0cd5 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.html @@ -0,0 +1,2732 @@ + + + + + + + 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:545115147.4 %
Date:2023-12-06 07:59:45Functions:313881.6 %
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             :   FLY_THERE_STATE,
+      73             :   LANDING_STATE,
+      74             : 
+      75             : } LandingStates_t;
+      76             : 
+      77             : const char* state_names[3] = {
+      78             : 
+      79             :     "IDLING", "FLYING HOME", "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             : 
+     139             :   // service callbacks
+     140             :   bool callbackTakeoff(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     141             :   bool callbackLand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     142             :   bool callbackLandHome(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     143             :   bool callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     144             : 
+     145             :   // service clients
+     146             :   mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>                sch_takeoff_;
+     147             :   mrs_lib::ServiceClientHandler<mrs_msgs::String>              sch_switch_tracker_;
+     148             :   mrs_lib::ServiceClientHandler<mrs_msgs::String>              sch_switch_controller_;
+     149             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_land_;
+     150             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_eland_;
+     151             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_ehover_;
+     152             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_control_callbacks_;
+     153             :   mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> sch_emergency_reference_;
+     154             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_arm_;
+     155             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_pirouette_;
+     156             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_odometry_callbacks_;
+     157             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_ungrip_;
+     158             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_toggle_control_output_;
+     159             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_offboard_;
+     160             : 
+     161             :   // service client wrappers
+     162             :   bool takeoffSrv(void);
+     163             :   bool switchTrackerSrv(const std::string& tracker);
+     164             :   bool switchControllerSrv(const std::string& controller);
+     165             :   bool landSrv(void);
+     166             :   bool elandSrv(void);
+     167             :   bool ehoverSrv(void);
+     168             :   void disarmSrv(void);
+     169             :   bool emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal);
+     170             :   void setOdometryCallbacksSrv(const bool& input);
+     171             :   void setControlCallbacksSrv(const bool& input);
+     172             :   void ungripSrv(void);
+     173             :   bool toggleControlOutput(const bool& input);
+     174             :   void pirouetteSrv(void);
+     175             :   bool offboardSrv(const bool in);
+     176             : 
+     177             :   ros::Timer timer_takeoff_;
+     178             :   ros::Timer timer_max_height_;
+     179             :   ros::Timer timer_min_height_;
+     180             :   ros::Timer timer_landing_;
+     181             :   ros::Timer timer_maxthrottle_;
+     182             :   ros::Timer timer_flighttime_;
+     183             :   ros::Timer timer_diagnostics_;
+     184             :   ros::Timer timer_midair_activation_;
+     185             : 
+     186             :   // timer callbacks
+     187             :   void timerLanding(const ros::TimerEvent& event);
+     188             :   void timerTakeoff(const ros::TimerEvent& event);
+     189             :   void timerMaxHeight(const ros::TimerEvent& event);
+     190             :   void timerMinHeight(const ros::TimerEvent& event);
+     191             :   void timerFlightTime(const ros::TimerEvent& event);
+     192             :   void timerMaxthrottle(const ros::TimerEvent& event);
+     193             :   void timerDiagnostics(const ros::TimerEvent& event);
+     194             : 
+     195             :   // publishers
+     196             :   mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics> ph_diag_;
+     197             : 
+     198             :   // max height checking
+     199             :   bool              _max_height_enabled_ = false;
+     200             :   double            _max_height_checking_rate_;
+     201             :   double            _max_height_offset_;
+     202             :   std::atomic<bool> fixing_max_height_ = false;
+     203             : 
+     204             :   // min height checking
+     205             :   bool              _min_height_enabled_ = false;
+     206             :   double            _min_height_checking_rate_;
+     207             :   double            _min_height_offset_;
+     208             :   double            _min_height_;
+     209             :   std::atomic<bool> fixing_min_height_ = false;
+     210             : 
+     211             :   // mass estimation during landing
+     212             :   double    throttle_mass_estimate_;
+     213             :   bool      throttle_under_threshold_ = false;
+     214             :   ros::Time throttle_mass_estimate_first_time_;
+     215             : 
+     216             :   // velocity during landing
+     217             :   bool      velocity_under_threshold_ = false;
+     218             :   ros::Time velocity_under_threshold_first_time_;
+     219             : 
+     220             :   bool _gain_manager_required_       = false;
+     221             :   bool _constraint_manager_required_ = false;
+     222             : 
+     223             :   std::tuple<bool, std::string> landImpl(void);
+     224             :   std::tuple<bool, std::string> landWithDescendImpl(void);
+     225             :   std::tuple<bool, std::string> midairActivationImpl(void);
+     226             : 
+     227             :   // saved takeoff coordinates and allows to land there again
+     228             :   mrs_msgs::ReferenceStamped land_there_reference_;
+     229             :   std::mutex                 mutex_land_there_reference_;
+     230             : 
+     231             :   // to which height to takeoff
+     232             :   double _takeoff_height_;
+     233             : 
+     234             :   std::atomic<bool> takeoff_successful_ = false;
+     235             : 
+     236             :   // names of important trackers
+     237             :   std::string _null_tracker_name_;
+     238             : 
+     239             :   // takeoff timer
+     240             :   double     _takeoff_timer_rate_;
+     241             :   bool       takingoff_            = false;
+     242             :   int        number_of_takeoffs_   = 0;
+     243             :   double     last_mass_difference_ = 0;
+     244             :   std::mutex mutex_last_mass_difference_;
+     245             :   bool       waiting_for_takeoff_ = false;
+     246             : 
+     247             :   // after takeoff
+     248             :   std::string _after_takeoff_tracker_name_;
+     249             :   std::string _after_takeoff_controller_name_;
+     250             :   std::string _takeoff_tracker_name_;
+     251             :   std::string _takeoff_controller_name_;
+     252             :   bool        _after_takeoff_pirouette_ = false;
+     253             : 
+     254             :   // Landing timer
+     255             :   std::string _landing_tracker_name_;
+     256             :   std::string _landing_controller_name_;
+     257             :   double      _landing_cutoff_mass_factor_;
+     258             :   double      _landing_cutoff_mass_timeout_;
+     259             :   double      _landing_timer_rate_;
+     260             :   double      _landing_descend_height_;
+     261             :   bool        landing_ = false;
+     262             :   double      _uav_mass_;
+     263             :   double      _g_;
+     264             :   double      landing_uav_mass_;
+     265             :   bool        _landing_disarm_ = false;
+     266             :   double      _landing_tracking_tolerance_translation_;
+     267             :   double      _landing_tracking_tolerance_heading_;
+     268             : 
+     269             :   // diagnostics timer
+     270             :   double _diagnostics_timer_rate_;
+     271             : 
+     272             :   mrs_lib::quadratic_throttle_model::MotorParams_t _throttle_model_;
+     273             : 
+     274             :   // landing state machine states
+     275             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     276             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     277             : 
+     278             :   // timer for checking max flight time
+     279             :   double     _flighttime_timer_rate_;
+     280             :   double     _flighttime_max_time_;
+     281             :   bool       _flighttime_timer_enabled_ = false;
+     282             :   double     flighttime_                = 0;
+     283             :   std::mutex mutex_flighttime_;
+     284             : 
+     285             :   // timer for checking maximum throttle
+     286             :   bool      _maxthrottle_timer_enabled_ = false;
+     287             :   double    _maxthrottle_timer_rate_;
+     288             :   double    _maxthrottle_max_throttle_;
+     289             :   double    _maxthrottle_eland_timeout_;
+     290             :   double    _maxthrottle_ungrip_timeout_;
+     291             :   bool      maxthrottle_above_threshold_ = false;
+     292             :   ros::Time maxthrottle_first_time_;
+     293             : 
+     294             :   // profiler
+     295             :   mrs_lib::Profiler profiler_;
+     296             :   bool              _profiler_enabled_ = false;
+     297             : 
+     298             :   // midair activation
+     299             :   void      timerMidairActivation(const ros::TimerEvent& event);
+     300             :   bool      callbackMidairActivation(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     301             :   ros::Time midair_activation_started_;
+     302             : 
+     303             :   double      _midair_activation_timer_rate_;
+     304             :   std::string _midair_activation_during_controller_;
+     305             :   std::string _midair_activation_during_tracker_;
+     306             :   std::string _midair_activation_after_controller_;
+     307             :   std::string _midair_activation_after_tracker_;
+     308             : 
+     309             :   void changeLandingState(LandingStates_t new_state);
+     310             : };
+     311             : 
+     312             : //}
+     313             : 
+     314             : /* onInit() //{ */
+     315             : 
+     316          42 : void UavManager::onInit() {
+     317          42 :   preinitialize();
+     318          42 : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : /* preinitialize() //{ */
+     323             : 
+     324          42 : void UavManager::preinitialize(void) {
+     325             : 
+     326          42 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     327             : 
+     328          42 :   ros::Time::waitForValid();
+     329             : 
+     330          42 :   mrs_lib::SubscribeHandlerOptions shopts;
+     331          42 :   shopts.nh                 = nh_;
+     332          42 :   shopts.node_name          = "ControlManager";
+     333          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     334          42 :   shopts.threadsafe         = true;
+     335          42 :   shopts.autostart          = true;
+     336          42 :   shopts.queue_size         = 10;
+     337          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     338             : 
+     339          42 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     340             : 
+     341          42 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &UavManager::timerHwApiCapabilities, this);
+     342          42 : }
+     343             : 
+     344             : //}
+     345             : 
+     346             : /* initialize() //{ */
+     347             : 
+     348          42 : void UavManager::initialize() {
+     349             : 
+     350          42 :   ROS_INFO("[UavManager]: initializing");
+     351             : 
+     352          84 :   mrs_lib::ParamLoader param_loader(nh_, "UavManager");
+     353             : 
+     354          84 :   std::string custom_config_path;
+     355          84 :   std::string platform_config_path;
+     356          84 :   std::string world_config_path;
+     357             : 
+     358          42 :   param_loader.loadParam("custom_config", custom_config_path);
+     359          42 :   param_loader.loadParam("platform_config", platform_config_path);
+     360          42 :   param_loader.loadParam("world_config", world_config_path);
+     361             : 
+     362          42 :   if (custom_config_path != "") {
+     363          42 :     param_loader.addYamlFile(custom_config_path);
+     364             :   }
+     365             : 
+     366          42 :   if (platform_config_path != "") {
+     367          42 :     param_loader.addYamlFile(platform_config_path);
+     368             :   }
+     369             : 
+     370          42 :   if (world_config_path != "") {
+     371          42 :     param_loader.addYamlFile(world_config_path);
+     372             :   }
+     373             : 
+     374          42 :   param_loader.addYamlFileFromParam("private_config");
+     375          42 :   param_loader.addYamlFileFromParam("public_config");
+     376             : 
+     377          84 :   const std::string yaml_prefix = "mrs_uav_managers/uav_manager/";
+     378             : 
+     379             :   // params passed from the launch file are not prefixed
+     380          42 :   param_loader.loadParam("uav_name", _uav_name_);
+     381          42 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     382          42 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     383          42 :   param_loader.loadParam("g", _g_);
+     384             : 
+     385             :   // motor params are also not prefixed, since they are common to more nodes
+     386          42 :   param_loader.loadParam("motor_params/n_motors", _throttle_model_.n_motors);
+     387          42 :   param_loader.loadParam("motor_params/a", _throttle_model_.A);
+     388          42 :   param_loader.loadParam("motor_params/b", _throttle_model_.B);
+     389             : 
+     390          42 :   param_loader.loadParam(yaml_prefix + "null_tracker", _null_tracker_name_);
+     391             : 
+     392          42 :   param_loader.loadParam(yaml_prefix + "takeoff/rate", _takeoff_timer_rate_);
+     393          42 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/tracker", _after_takeoff_tracker_name_);
+     394          42 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/controller", _after_takeoff_controller_name_);
+     395          42 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/pirouette", _after_takeoff_pirouette_);
+     396          42 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/controller", _takeoff_controller_name_);
+     397          42 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/tracker", _takeoff_tracker_name_);
+     398          42 :   param_loader.loadParam(yaml_prefix + "takeoff/takeoff_height", _takeoff_height_);
+     399             : 
+     400          42 :   param_loader.loadParam(yaml_prefix + "landing/rate", _landing_timer_rate_);
+     401          42 :   param_loader.loadParam(yaml_prefix + "landing/landing_tracker", _landing_tracker_name_);
+     402          42 :   param_loader.loadParam(yaml_prefix + "landing/landing_controller", _landing_controller_name_);
+     403          42 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_mass_factor", _landing_cutoff_mass_factor_);
+     404          42 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_timeout", _landing_cutoff_mass_timeout_);
+     405          42 :   param_loader.loadParam(yaml_prefix + "landing/disarm", _landing_disarm_);
+     406          42 :   param_loader.loadParam(yaml_prefix + "landing/descend_height", _landing_descend_height_);
+     407          42 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/translation", _landing_tracking_tolerance_translation_);
+     408          42 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/heading", _landing_tracking_tolerance_heading_);
+     409             : 
+     410          42 :   param_loader.loadParam(yaml_prefix + "midair_activation/rate", _midair_activation_timer_rate_);
+     411          42 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/controller", _midair_activation_during_controller_);
+     412          42 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/tracker", _midair_activation_during_tracker_);
+     413          42 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/controller", _midair_activation_after_controller_);
+     414          42 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/tracker", _midair_activation_after_tracker_);
+     415             : 
+     416          42 :   param_loader.loadParam(yaml_prefix + "max_height_checking/enabled", _max_height_enabled_);
+     417          42 :   param_loader.loadParam(yaml_prefix + "max_height_checking/rate", _max_height_checking_rate_);
+     418          42 :   param_loader.loadParam(yaml_prefix + "max_height_checking/safety_height_offset", _max_height_offset_);
+     419             : 
+     420          42 :   param_loader.loadParam(yaml_prefix + "min_height_checking/enabled", _min_height_enabled_);
+     421          42 :   param_loader.loadParam(yaml_prefix + "min_height_checking/rate", _min_height_checking_rate_);
+     422          42 :   param_loader.loadParam(yaml_prefix + "min_height_checking/safety_height_offset", _min_height_offset_);
+     423          42 :   param_loader.loadParam(yaml_prefix + "min_height_checking/min_height", _min_height_);
+     424             : 
+     425          42 :   param_loader.loadParam(yaml_prefix + "require_gain_manager", _gain_manager_required_);
+     426          42 :   param_loader.loadParam(yaml_prefix + "require_constraint_manager", _constraint_manager_required_);
+     427             : 
+     428          42 :   param_loader.loadParam(yaml_prefix + "flight_timer/enabled", _flighttime_timer_enabled_);
+     429          42 :   param_loader.loadParam(yaml_prefix + "flight_timer/rate", _flighttime_timer_rate_);
+     430          42 :   param_loader.loadParam(yaml_prefix + "flight_timer/max_time", _flighttime_max_time_);
+     431             : 
+     432          42 :   param_loader.loadParam(yaml_prefix + "max_throttle/enabled", _maxthrottle_timer_enabled_);
+     433          42 :   param_loader.loadParam(yaml_prefix + "max_throttle/rate", _maxthrottle_timer_rate_);
+     434          42 :   param_loader.loadParam(yaml_prefix + "max_throttle/max_throttle", _maxthrottle_max_throttle_);
+     435          42 :   param_loader.loadParam(yaml_prefix + "max_throttle/eland_timeout", _maxthrottle_eland_timeout_);
+     436          42 :   param_loader.loadParam(yaml_prefix + "max_throttle/ungrip_timeout", _maxthrottle_ungrip_timeout_);
+     437             : 
+     438          42 :   param_loader.loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_timer_rate_);
+     439             : 
+     440             :   // | ------------------- scope timer logger ------------------- |
+     441             : 
+     442          42 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     443         126 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     444          42 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     445             : 
+     446          42 :   if (!param_loader.loadedSuccessfully()) {
+     447           0 :     ROS_ERROR("[UavManager]: Could not load all parameters!");
+     448           0 :     ros::shutdown();
+     449             :   }
+     450             : 
+     451             :   // | --------------------- tf transformer --------------------- |
+     452             : 
+     453          42 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+     454          42 :   transformer_->setDefaultPrefix(_uav_name_);
+     455          42 :   transformer_->retryLookupNewest(true);
+     456             : 
+     457             :   // | ----------------------- subscribers ---------------------- |
+     458             : 
+     459          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+     460          42 :   shopts.nh                 = nh_;
+     461          42 :   shopts.node_name          = "UavManager";
+     462          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     463          42 :   shopts.threadsafe         = true;
+     464          42 :   shopts.autostart          = true;
+     465          42 :   shopts.queue_size         = 10;
+     466          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     467             : 
+     468          42 :   sh_controller_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics>(shopts, "controller_diagnostics_in");
+     469          42 :   sh_odometry_               = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &UavManager::callbackOdometry, this);
+     470          42 :   sh_estimation_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "odometry_diagnostics_in");
+     471          42 :   sh_control_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     472          42 :   sh_mass_estimate_          = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "mass_estimate_in");
+     473          42 :   sh_throttle_               = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "throttle_in");
+     474          42 :   sh_height_                 = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_in");
+     475          42 :   sh_hw_api_status_          = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in");
+     476          42 :   sh_gains_diag_             = mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>(shopts, "gain_manager_diagnostics_in");
+     477          42 :   sh_constraints_diag_       = mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics>(shopts, "constraint_manager_diagnostics_in");
+     478          42 :   sh_hw_api_gnss_            = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "hw_api_gnss_in", &UavManager::callbackHwApiGNSS, this);
+     479          42 :   sh_max_height_             = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_height_in");
+     480          42 :   sh_tracker_cmd_            = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts, "tracker_cmd_in");
+     481             : 
+     482             :   // | ----------------------- publishers ----------------------- |
+     483             : 
+     484          42 :   ph_diag_ = mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     485             : 
+     486             :   // | --------------------- service servers -------------------- |
+     487             : 
+     488          42 :   service_server_takeoff_           = nh_.advertiseService("takeoff_in", &UavManager::callbackTakeoff, this);
+     489          42 :   service_server_land_              = nh_.advertiseService("land_in", &UavManager::callbackLand, this);
+     490          42 :   service_server_land_home_         = nh_.advertiseService("land_home_in", &UavManager::callbackLandHome, this);
+     491          42 :   service_server_land_there_        = nh_.advertiseService("land_there_in", &UavManager::callbackLandThere, this);
+     492          42 :   service_server_midair_activation_ = nh_.advertiseService("midair_activation_in", &UavManager::callbackMidairActivation, this);
+     493             : 
+     494             :   // | --------------------- service clients -------------------- |
+     495             : 
+     496          42 :   sch_takeoff_               = mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>(nh_, "takeoff_out");
+     497          42 :   sch_land_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "land_out");
+     498          42 :   sch_eland_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+     499          42 :   sch_ehover_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ehover_out");
+     500          42 :   sch_switch_tracker_        = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_tracker_out");
+     501          42 :   sch_switch_controller_     = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_controller_out");
+     502          42 :   sch_emergency_reference_   = mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>(nh_, "emergency_reference_out");
+     503          42 :   sch_control_callbacks_     = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "enable_callbacks_out");
+     504          42 :   sch_arm_                   = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "arm_out");
+     505          42 :   sch_pirouette_             = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "pirouette_out");
+     506          42 :   sch_odometry_callbacks_    = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+     507          42 :   sch_ungrip_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+     508          42 :   sch_toggle_control_output_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "toggle_control_output_out");
+     509          42 :   sch_offboard_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "offboard_out");
+     510             : 
+     511             :   // | ---------------------- state machine --------------------- |
+     512             : 
+     513          42 :   current_state_landing_ = IDLE_STATE;
+     514             : 
+     515             :   // | ------------------------ profiler ------------------------ |
+     516             : 
+     517          42 :   profiler_ = mrs_lib::Profiler(nh_, "UavManager", _profiler_enabled_);
+     518             : 
+     519             :   // | ------------------------- timers ------------------------- |
+     520             : 
+     521          42 :   timer_landing_           = nh_.createTimer(ros::Rate(_landing_timer_rate_), &UavManager::timerLanding, this, false, false);
+     522          42 :   timer_takeoff_           = nh_.createTimer(ros::Rate(_takeoff_timer_rate_), &UavManager::timerTakeoff, this, false, false);
+     523          42 :   timer_flighttime_        = nh_.createTimer(ros::Rate(_flighttime_timer_rate_), &UavManager::timerFlightTime, this, false, false);
+     524          42 :   timer_diagnostics_       = nh_.createTimer(ros::Rate(_diagnostics_timer_rate_), &UavManager::timerDiagnostics, this);
+     525          42 :   timer_midair_activation_ = nh_.createTimer(ros::Rate(_midair_activation_timer_rate_), &UavManager::timerMidairActivation, this, false, false);
+     526          84 :   timer_max_height_        = nh_.createTimer(ros::Rate(_max_height_checking_rate_), &UavManager::timerMaxHeight, this, false,
+     527          84 :                                       _max_height_enabled_ && hw_api_capabilities_.produces_distance_sensor);
+     528          84 :   timer_min_height_        = nh_.createTimer(ros::Rate(_min_height_checking_rate_), &UavManager::timerMinHeight, this, false,
+     529          84 :                                       _min_height_enabled_ && hw_api_capabilities_.produces_distance_sensor);
+     530             : 
+     531          40 :   bool should_check_throttle = hw_api_capabilities_.accepts_actuator_cmd || hw_api_capabilities_.accepts_control_group_cmd ||
+     532          82 :                                hw_api_capabilities_.accepts_attitude_rate_cmd || hw_api_capabilities_.accepts_attitude_cmd;
+     533             : 
+     534             :   timer_maxthrottle_ =
+     535          42 :       nh_.createTimer(ros::Rate(_maxthrottle_timer_rate_), &UavManager::timerMaxthrottle, this, false, _maxthrottle_timer_enabled_ && should_check_throttle);
+     536             : 
+     537             :   // | ----------------------- finish init ---------------------- |
+     538             : 
+     539          42 :   is_initialized_ = true;
+     540             : 
+     541          42 :   ROS_INFO("[UavManager]: initialized");
+     542             : 
+     543          42 :   ROS_DEBUG("[UavManager]: debug output is enabled");
+     544          42 : }
+     545             : 
+     546             : //}
+     547             : 
+     548             : //}
+     549             : 
+     550             : // | ---------------------- state machine --------------------- |
+     551             : 
+     552             : /* //{ changeLandingState() */
+     553             : 
+     554           4 : void UavManager::changeLandingState(LandingStates_t new_state) {
+     555             : 
+     556           4 :   previous_state_landing_ = current_state_landing_;
+     557           4 :   current_state_landing_  = new_state;
+     558             : 
+     559           4 :   switch (current_state_landing_) {
+     560             : 
+     561           2 :     case LANDING_STATE: {
+     562             : 
+     563           2 :       if (sh_mass_estimate_.hasMsg() && (ros::Time::now() - sh_mass_estimate_.lastMsgTime()).toSec() < 1.0) {
+     564             : 
+     565             :         // copy member variables
+     566           2 :         auto mass_esimtate = sh_mass_estimate_.getMsg();
+     567             : 
+     568           2 :         landing_uav_mass_ = mass_esimtate->data;
+     569             :       }
+     570             : 
+     571           2 :       break;
+     572             :     };
+     573             : 
+     574           2 :     default: {
+     575           2 :       break;
+     576             :     }
+     577             :   }
+     578             : 
+     579             :   // just for ROS_INFO
+     580           4 :   ROS_INFO("[UavManager]: Switching landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+     581           4 : }
+     582             : 
+     583             : //}
+     584             : 
+     585             : // --------------------------------------------------------------
+     586             : // |                           timers                           |
+     587             : // --------------------------------------------------------------
+     588             : 
+     589             : /* timerHwApiCapabilities() //{ */
+     590             : 
+     591          46 : void UavManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+     592             : 
+     593          92 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", 1.0, 1.0, event);
+     594          92 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+     595             : 
+     596          46 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+     597           4 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+     598           4 :     return;
+     599             :   }
+     600             : 
+     601          42 :   hw_api_capabilities_ = *sh_hw_api_capabilities_.getMsg();
+     602             : 
+     603          42 :   ROS_INFO("[ControlManager]: got HW API capabilities, initializing");
+     604             : 
+     605          42 :   initialize();
+     606             : 
+     607          42 :   timer_hw_api_capabilities_.stop();
+     608             : }
+     609             : 
+     610             : //}
+     611             : 
+     612             : /* //{ timerLanding() */
+     613             : 
+     614         177 : void UavManager::timerLanding(const ros::TimerEvent& event) {
+     615             : 
+     616         177 :   if (!is_initialized_)
+     617           0 :     return;
+     618             : 
+     619         354 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerLanding", _landing_timer_rate_, 0.1, event);
+     620         354 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerLanding", scope_timer_logger_, scope_timer_enabled_);
+     621             : 
+     622         177 :   auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+     623             : 
+     624             :   // copy member variables
+     625         177 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     626         177 :   auto odometry                    = sh_odometry_.getMsg();
+     627         177 :   auto tracker_cmd                 = sh_tracker_cmd_.getMsg();
+     628             : 
+     629         177 :   std::optional<double> desired_throttle;
+     630             : 
+     631         177 :   if (sh_throttle_.hasMsg() && (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() < 1.0) {
+     632         177 :     desired_throttle = sh_throttle_.getMsg()->data;
+     633             :   }
+     634             : 
+     635         177 :   auto res = transformer_->transformSingle(land_there_reference, odometry->header.frame_id);
+     636             : 
+     637         177 :   mrs_msgs::ReferenceStamped land_there_current_frame;
+     638             : 
+     639         177 :   if (res) {
+     640         177 :     land_there_current_frame = res.value();
+     641             :   } else {
+     642             : 
+     643           0 :     ROS_ERROR("[UavManager]: could not transform the reference into the current frame! land by yourself pls.");
+     644           0 :     return;
+     645             :   }
+     646             : 
+     647         177 :   if (current_state_landing_ == IDLE_STATE) {
+     648             : 
+     649           0 :     return;
+     650             : 
+     651         177 :   } else if (current_state_landing_ == FLY_THERE_STATE) {
+     652             : 
+     653           0 :     auto [pos_x, pos_y, pos_z] = mrs_lib::getPosition(*tracker_cmd);
+     654           0 :     auto [ref_x, ref_y, ref_z] = mrs_lib::getPosition(land_there_current_frame);
+     655             : 
+     656           0 :     double pos_heading = tracker_cmd->heading;
+     657             : 
+     658           0 :     double ref_heading = 0;
+     659             :     try {
+     660           0 :       ref_heading = mrs_lib::getHeading(land_there_current_frame);
+     661             :     }
+     662           0 :     catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     663           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     664           0 :       return;
+     665             :     }
+     666             : 
+     667           0 :     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_ &&
+     668           0 :         fabs(radians::diff(pos_heading, ref_heading)) < _landing_tracking_tolerance_heading_) {
+     669             : 
+     670           0 :       auto [success, message] = landWithDescendImpl();
+     671             : 
+     672           0 :       if (!success) {
+     673             : 
+     674           0 :         ROS_ERROR_THROTTLE(1.0, "[UavManager]: call for landing failed: '%s'", message.c_str());
+     675             :       }
+     676             : 
+     677           0 :     } else if (!control_manager_diagnostics->tracker_status.have_goal && control_manager_diagnostics->flying_normally) {
+     678             : 
+     679           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: the tracker does not have a goal while flying home, setting the reference again");
+     680             : 
+     681           0 :       mrs_msgs::ReferenceStamped reference_out;
+     682             : 
+     683             :       {
+     684           0 :         std::scoped_lock lock(mutex_land_there_reference_);
+     685             : 
+     686             :         // get the current altitude in land_there_reference_.header.frame_id;
+     687           0 :         geometry_msgs::PoseStamped current_pose;
+     688           0 :         current_pose.header.stamp     = ros::Time::now();
+     689           0 :         current_pose.header.frame_id  = _uav_name_ + "/fcu";
+     690           0 :         current_pose.pose.position.x  = 0;
+     691           0 :         current_pose.pose.position.y  = 0;
+     692           0 :         current_pose.pose.position.z  = 0;
+     693           0 :         current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     694             : 
+     695           0 :         auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+     696             : 
+     697           0 :         if (response) {
+     698             : 
+     699           0 :           land_there_reference_.reference.position.z = response.value().pose.position.z;
+     700           0 :           ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+     701             : 
+     702             :         } else {
+     703             : 
+     704           0 :           std::stringstream ss;
+     705           0 :           ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+     706           0 :           ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+     707             :         }
+     708             : 
+     709           0 :         reference_out.header.frame_id = land_there_reference_.header.frame_id;
+     710           0 :         reference_out.header.stamp    = ros::Time::now();
+     711           0 :         reference_out.reference       = land_there_reference_.reference;
+     712             :       }
+     713             : 
+     714           0 :       emergencyReferenceSrv(reference_out);
+     715             :     }
+     716             : 
+     717         177 :   } else if (current_state_landing_ == LANDING_STATE) {
+     718             : 
+     719             :     // we should not attempt to finish the landing if some other tracked was activated
+     720         177 :     if (_landing_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+     721             : 
+     722         177 :       if (desired_throttle) {
+     723             : 
+     724             :         // recalculate the mass based on the throttle
+     725         177 :         throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(_throttle_model_, desired_throttle.value()) / _g_;
+     726         177 :         ROS_INFO_THROTTLE(1.0, "[UavManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+     727             : 
+     728             :         // condition for automatic motor turn off
+     729         177 :         if (((throttle_mass_estimate_ < _landing_cutoff_mass_factor_ * landing_uav_mass_) || desired_throttle < 0.01)) {
+     730             : 
+     731          44 :           if (!throttle_under_threshold_) {
+     732             : 
+     733           2 :             throttle_mass_estimate_first_time_ = ros::Time::now();
+     734           2 :             throttle_under_threshold_          = true;
+     735             :           }
+     736             : 
+     737          44 :           ROS_INFO_THROTTLE(0.5, "[UavManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+     738             : 
+     739             :         } else {
+     740             : 
+     741         133 :           throttle_under_threshold_ = false;
+     742             :         }
+     743             : 
+     744         177 :         if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _landing_cutoff_mass_timeout_)) {
+     745             : 
+     746           2 :           switchTrackerSrv(_null_tracker_name_);
+     747             : 
+     748           2 :           setControlCallbacksSrv(true);
+     749             : 
+     750           2 :           if (_landing_disarm_) {
+     751             : 
+     752           2 :             ROS_INFO("[UavManager]: disarming after landing");
+     753             : 
+     754           2 :             disarmSrv();
+     755             :           }
+     756             : 
+     757           2 :           changeLandingState(IDLE_STATE);
+     758             : 
+     759           2 :           ROS_INFO("[UavManager]: landing finished");
+     760             : 
+     761           2 :           timer_landing_.stop();
+     762             :         }
+     763             : 
+     764             :       } else {
+     765             : 
+     766           0 :         auto odometry = sh_odometry_.getMsg();
+     767             : 
+     768           0 :         double z_vel = odometry->twist.twist.linear.z;
+     769             : 
+     770           0 :         ROS_INFO_THROTTLE(1.0, "[UavManager]: landing: z-velocity: %.2f", z_vel);
+     771             : 
+     772             :         // condition for automatic motor turn off
+     773           0 :         if (z_vel > -0.1) {
+     774             : 
+     775           0 :           if (!velocity_under_threshold_) {
+     776             : 
+     777           0 :             velocity_under_threshold_first_time_ = ros::Time::now();
+     778           0 :             velocity_under_threshold_            = true;
+     779             :           }
+     780             : 
+     781           0 :           ROS_INFO_THROTTLE(0.5, "[UavManager]: velocity over threshold for %.2f s", (ros::Time::now() - velocity_under_threshold_first_time_).toSec());
+     782             : 
+     783             :         } else {
+     784             : 
+     785           0 :           velocity_under_threshold_ = false;
+     786             :         }
+     787             : 
+     788           0 :         if (velocity_under_threshold_ && ((ros::Time::now() - velocity_under_threshold_first_time_).toSec() > 3.0)) {
+     789             : 
+     790           0 :           switchTrackerSrv(_null_tracker_name_);
+     791             : 
+     792           0 :           setControlCallbacksSrv(true);
+     793             : 
+     794           0 :           if (_landing_disarm_) {
+     795             : 
+     796           0 :             ROS_INFO("[UavManager]: disarming after landing");
+     797             : 
+     798           0 :             disarmSrv();
+     799             :           }
+     800             : 
+     801           0 :           changeLandingState(IDLE_STATE);
+     802             : 
+     803           0 :           ROS_INFO("[UavManager]: landing finished");
+     804             : 
+     805           0 :           timer_landing_.stop();
+     806             :         }
+     807             :       }
+     808             : 
+     809             :     } else {
+     810             : 
+     811           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: incorrect tracker detected during landing!");
+     812             :     }
+     813             :   }
+     814             : }
+     815             : 
+     816             : //}
+     817             : 
+     818             : /* //{ timerTakeoff() */
+     819             : 
+     820         287 : void UavManager::timerTakeoff(const ros::TimerEvent& event) {
+     821             : 
+     822         287 :   if (!is_initialized_)
+     823           0 :     return;
+     824             : 
+     825         574 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerTakeoff", _takeoff_timer_rate_, 0.1, event);
+     826         574 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerTakeoff", scope_timer_logger_, scope_timer_enabled_);
+     827             : 
+     828         287 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     829             : 
+     830         287 :   if (waiting_for_takeoff_) {
+     831             : 
+     832           6 :     if (control_manager_diagnostics->active_tracker == _takeoff_tracker_name_ && control_manager_diagnostics->tracker_status.have_goal) {
+     833             : 
+     834           6 :       waiting_for_takeoff_ = false;
+     835             :     } else {
+     836             : 
+     837           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: waiting for takeoff confirmation from the ControlManager");
+     838           0 :       return;
+     839             :     }
+     840             :   }
+     841             : 
+     842         287 :   if (takingoff_) {
+     843             : 
+     844         287 :     if (control_manager_diagnostics->active_tracker != _takeoff_tracker_name_ || !control_manager_diagnostics->tracker_status.have_goal) {
+     845             : 
+     846           6 :       ROS_INFO("[UavManager]: take off finished, switching to %s", _after_takeoff_tracker_name_.c_str());
+     847             : 
+     848           6 :       switchTrackerSrv(_after_takeoff_tracker_name_);
+     849             : 
+     850           6 :       switchControllerSrv(_after_takeoff_controller_name_);
+     851             : 
+     852           6 :       setOdometryCallbacksSrv(true);
+     853             : 
+     854           6 :       if (_after_takeoff_pirouette_) {
+     855             : 
+     856           0 :         pirouetteSrv();
+     857             :       }
+     858             : 
+     859           6 :       timer_takeoff_.stop();
+     860             :     }
+     861             :   }
+     862             : }
+     863             : 
+     864             : //}
+     865             : 
+     866             : /* //{ timerMaxHeight() */
+     867             : 
+     868        6413 : void UavManager::timerMaxHeight(const ros::TimerEvent& event) {
+     869             : 
+     870        6413 :   if (!is_initialized_)
+     871        3958 :     return;
+     872             : 
+     873       12826 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxHeight", _max_height_checking_rate_, 0.1, event);
+     874       12826 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxHeight", scope_timer_logger_, scope_timer_enabled_);
+     875             : 
+     876        6413 :   if (!sh_max_height_.hasMsg() || !sh_height_.hasMsg() || !sh_odometry_.hasMsg()) {
+     877        2442 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: maxHeightTimer() not spinning, missing data");
+     878        2442 :     return;
+     879             :   }
+     880             : 
+     881        3971 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     882             : 
+     883        3971 :   if (!control_manager_diag->flying_normally) {
+     884        1516 :     return;
+     885             :   }
+     886             : 
+     887        2455 :   auto   odometry = sh_odometry_.getMsg();
+     888        2455 :   double height   = sh_height_.getMsg()->value;
+     889             : 
+     890             :   // transform max z to the height frame
+     891        2455 :   geometry_msgs::PointStamped point;
+     892        2455 :   point.header  = sh_max_height_.getMsg()->header;
+     893        2455 :   point.point.z = sh_max_height_.getMsg()->value;
+     894             : 
+     895        2455 :   auto res = transformer_->transformSingle(point, sh_height_.getMsg()->header.frame_id);
+     896             : 
+     897             :   double max_z_in_height;
+     898             : 
+     899        2455 :   if (res) {
+     900        2455 :     max_z_in_height = res->point.z;
+     901             :   } else {
+     902           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: timerMaxHeight() not working, cannot transform max z to the height frame");
+     903           0 :     return;
+     904             :   }
+     905             : 
+     906        2455 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     907             : 
+     908        2455 :   double odometry_heading = 0;
+     909             :   try {
+     910        2455 :     odometry_heading = mrs_lib::getHeading(odometry);
+     911             :   }
+     912           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     913           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     914           0 :     return;
+     915             :   }
+     916             : 
+     917        2455 :   if (height > max_z_in_height) {
+     918             : 
+     919           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: max height exceeded: %.2f >  %.2f, triggering safety goto", height, max_z_in_height);
+     920             : 
+     921           0 :     mrs_msgs::ReferenceStamped reference_out;
+     922           0 :     reference_out.header.frame_id = odometry->header.frame_id;
+     923           0 :     reference_out.header.stamp    = ros::Time::now();
+     924             : 
+     925           0 :     reference_out.reference.position.x = odometry_x;
+     926           0 :     reference_out.reference.position.y = odometry_y;
+     927           0 :     reference_out.reference.position.z = odometry_z + ((max_z_in_height - _max_height_offset_) - height);
+     928             : 
+     929           0 :     reference_out.reference.heading = odometry_heading;
+     930             : 
+     931           0 :     setControlCallbacksSrv(false);
+     932             : 
+     933           0 :     bool success = emergencyReferenceSrv(reference_out);
+     934             : 
+     935           0 :     if (success) {
+     936             : 
+     937           0 :       ROS_INFO("[UavManager]: descending");
+     938             : 
+     939           0 :       fixing_max_height_ = true;
+     940             : 
+     941             :     } else {
+     942             : 
+     943           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not descend");
+     944             : 
+     945           0 :       setControlCallbacksSrv(true);
+     946             :     }
+     947             :   }
+     948             : 
+     949        2455 :   if (fixing_max_height_ && height < max_z_in_height) {
+     950             : 
+     951           0 :     setControlCallbacksSrv(true);
+     952             : 
+     953           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+     954             : 
+     955           0 :     fixing_max_height_ = false;
+     956             :   }
+     957             : }
+     958             : 
+     959             : //}
+     960             : 
+     961             : /* //{ timerMinHeight() */
+     962             : 
+     963        6413 : void UavManager::timerMinHeight(const ros::TimerEvent& event) {
+     964             : 
+     965        6413 :   if (!is_initialized_)
+     966        3958 :     return;
+     967             : 
+     968        6413 :   ROS_INFO_ONCE("[UavManager]: min height timer spinning");
+     969             : 
+     970       12826 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMinHeight", _min_height_checking_rate_, 0.1, event);
+     971       12826 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMinHeight", scope_timer_logger_, scope_timer_enabled_);
+     972             : 
+     973        6413 :   if (!sh_odometry_.hasMsg() || !sh_height_.hasMsg() || !sh_control_manager_diag_.hasMsg()) {
+     974        2434 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: minHeightTimer() not spinning, missing data");
+     975        2434 :     return;
+     976             :   }
+     977             : 
+     978        3979 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     979             : 
+     980        3979 :   if (!control_manager_diag->flying_normally) {
+     981        1524 :     return;
+     982             :   }
+     983             : 
+     984        2455 :   auto   odometry = sh_odometry_.getMsg();
+     985        2455 :   double height   = sh_height_.getMsg()->value;
+     986             : 
+     987        2455 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     988             : 
+     989        2455 :   double odometry_heading = 0;
+     990             :   try {
+     991        2455 :     odometry_heading = mrs_lib::getHeading(odometry);
+     992             :   }
+     993           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     994           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     995           0 :     return;
+     996             :   }
+     997             : 
+     998        2455 :   if (height < _min_height_) {
+     999             : 
+    1000           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: min height breached: %.2f < %.2f, triggering safety goto", height, _min_height_);
+    1001             : 
+    1002           0 :     mrs_msgs::ReferenceStamped reference_out;
+    1003           0 :     reference_out.header.frame_id = odometry->header.frame_id;
+    1004           0 :     reference_out.header.stamp    = ros::Time::now();
+    1005             : 
+    1006           0 :     reference_out.reference.position.x = odometry_x;
+    1007           0 :     reference_out.reference.position.y = odometry_y;
+    1008           0 :     reference_out.reference.position.z = odometry_z + ((_min_height_ + _min_height_offset_) - height);
+    1009             : 
+    1010           0 :     reference_out.reference.heading = odometry_heading;
+    1011             : 
+    1012           0 :     setControlCallbacksSrv(false);
+    1013             : 
+    1014           0 :     bool success = emergencyReferenceSrv(reference_out);
+    1015             : 
+    1016           0 :     if (success) {
+    1017             : 
+    1018           0 :       ROS_INFO("[UavManager]: ascending");
+    1019             : 
+    1020           0 :       fixing_min_height_ = true;
+    1021             : 
+    1022             :     } else {
+    1023             : 
+    1024           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not ascend");
+    1025             : 
+    1026           0 :       setControlCallbacksSrv(true);
+    1027             :     }
+    1028             :   }
+    1029             : 
+    1030        2455 :   if (fixing_min_height_ && height > _min_height_) {
+    1031             : 
+    1032           0 :     setControlCallbacksSrv(true);
+    1033             : 
+    1034           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+    1035             : 
+    1036           0 :     fixing_min_height_ = false;
+    1037             :   }
+    1038             : }
+    1039             : 
+    1040             : //}
+    1041             : 
+    1042             : /* //{ timerFlightTime() */
+    1043             : 
+    1044          46 : void UavManager::timerFlightTime(const ros::TimerEvent& event) {
+    1045             : 
+    1046          46 :   if (!is_initialized_)
+    1047           0 :     return;
+    1048             : 
+    1049         138 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFlightTime", _flighttime_timer_rate_, 0.1, event);
+    1050         138 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerFlightTime", scope_timer_logger_, scope_timer_enabled_);
+    1051             : 
+    1052          46 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1053             : 
+    1054          46 :   flighttime += 1.0 / _flighttime_timer_rate_;
+    1055             : 
+    1056          46 :   mrs_msgs::Float64 flight_time;
+    1057          46 :   flight_time.value = flighttime;
+    1058             : 
+    1059             :   // if enabled, start the timer for measuring the flight time
+    1060          46 :   if (_flighttime_timer_enabled_) {
+    1061             : 
+    1062           0 :     if (flighttime > _flighttime_max_time_) {
+    1063             : 
+    1064           0 :       flighttime = 0;
+    1065           0 :       timer_flighttime_.stop();
+    1066             : 
+    1067           0 :       ROS_INFO("[UavManager]: max flight time reached, landing");
+    1068             : 
+    1069           0 :       landImpl();
+    1070             :     }
+    1071             :   }
+    1072             : 
+    1073          46 :   mrs_lib::set_mutexed(mutex_flighttime_, flighttime, flighttime_);
+    1074             : }
+    1075             : 
+    1076             : //}
+    1077             : 
+    1078             : /* //{ timerMaxthrottle() */
+    1079             : 
+    1080       14795 : void UavManager::timerMaxthrottle(const ros::TimerEvent& event) {
+    1081             : 
+    1082       14795 :   if (!is_initialized_)
+    1083        4869 :     return;
+    1084             : 
+    1085       14795 :   if (!sh_throttle_.hasMsg() || (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() > 1.0) {
+    1086        4869 :     return;
+    1087             :   }
+    1088             : 
+    1089        9926 :   ROS_INFO_ONCE("[UavManager]: max throttle timer spinning");
+    1090             : 
+    1091       29778 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxthrottle", _maxthrottle_timer_rate_, 0.03, event);
+    1092       29778 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxthrottle", scope_timer_logger_, scope_timer_enabled_);
+    1093             : 
+    1094        9926 :   auto desired_throttle = sh_throttle_.getMsg()->data;
+    1095             : 
+    1096        9926 :   if (desired_throttle >= _maxthrottle_max_throttle_) {
+    1097             : 
+    1098           0 :     if (!maxthrottle_above_threshold_) {
+    1099             : 
+    1100           0 :       maxthrottle_first_time_      = ros::Time::now();
+    1101           0 :       maxthrottle_above_threshold_ = true;
+    1102           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: max throttle exceeded threshold (%.2f/%.2f)", desired_throttle, _maxthrottle_max_throttle_);
+    1103             : 
+    1104             :     } else {
+    1105             : 
+    1106           0 :       ROS_WARN_THROTTLE(0.1, "[UavManager]: throttle over threshold (%.2f/%.2f) for %.2f s", desired_throttle, _maxthrottle_max_throttle_,
+    1107             :                         (ros::Time::now() - maxthrottle_first_time_).toSec());
+    1108             :     }
+    1109             : 
+    1110             :   } else {
+    1111             : 
+    1112        9926 :     maxthrottle_above_threshold_ = false;
+    1113             :   }
+    1114             : 
+    1115        9926 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_ungrip_timeout_) {
+    1116             : 
+    1117           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, ungripping payload", desired_throttle,
+    1118             :                       _maxthrottle_max_throttle_, _maxthrottle_ungrip_timeout_);
+    1119             : 
+    1120           0 :     ungripSrv();
+    1121             :   }
+    1122             : 
+    1123        9926 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_eland_timeout_) {
+    1124             : 
+    1125           0 :     timer_maxthrottle_.stop();
+    1126             : 
+    1127           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, calling for emergency landing", desired_throttle,
+    1128             :                        _maxthrottle_max_throttle_, _maxthrottle_eland_timeout_);
+    1129             : 
+    1130           0 :     elandSrv();
+    1131             :   }
+    1132             : }
+    1133             : 
+    1134             : //}
+    1135             : 
+    1136             : /* //{ timerDiagnostics() */
+    1137             : 
+    1138         623 : void UavManager::timerDiagnostics(const ros::TimerEvent& event) {
+    1139             : 
+    1140         623 :   if (!is_initialized_)
+    1141           0 :     return;
+    1142             : 
+    1143        1869 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _maxthrottle_timer_rate_, 0.03, event);
+    1144        1869 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    1145             : 
+    1146         623 :   bool got_gps_est = false;
+    1147         623 :   bool got_rtk_est = false;
+    1148             : 
+    1149         623 :   if (sh_estimation_diagnostics_.hasMsg()) {  // get current position in lat-lon
+    1150             : 
+    1151         946 :     auto                     estimation_diag  = sh_estimation_diagnostics_.getMsg();
+    1152         473 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    1153             : 
+    1154         833 :     got_gps_est = std::find(state_estimators.begin(), state_estimators.end(), "gps_garmin") != state_estimators.end() ||
+    1155         360 :                   std::find(state_estimators.begin(), state_estimators.end(), "gps_baro") != state_estimators.end();
+    1156         473 :     got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    1157             :   }
+    1158             : 
+    1159        1246 :   mrs_msgs::UavManagerDiagnostics diag;
+    1160             : 
+    1161         623 :   diag.stamp    = ros::Time::now();
+    1162         623 :   diag.uav_name = _uav_name_;
+    1163             : 
+    1164         623 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1165             : 
+    1166             :   // fill in the acumulated flight time
+    1167         623 :   diag.flight_time = flighttime;
+    1168             : 
+    1169         623 :   if (sh_odometry_.hasMsg()) {  // get current position in lat-lon
+    1170             : 
+    1171         481 :     if (got_gps_est || got_rtk_est) {
+    1172             : 
+    1173         866 :       nav_msgs::Odometry odom = *sh_odometry_.getMsg();
+    1174             : 
+    1175         866 :       geometry_msgs::PoseStamped uav_pose;
+    1176         433 :       uav_pose.pose = mrs_lib::getPose(odom);
+    1177             : 
+    1178        1299 :       auto res = transformer_->transformSingle(uav_pose, "latlon_origin");
+    1179             : 
+    1180         433 :       if (res) {
+    1181         433 :         diag.cur_latitude  = res.value().pose.position.x;
+    1182         433 :         diag.cur_longitude = res.value().pose.position.y;
+    1183             :       }
+    1184             :     }
+    1185             :   }
+    1186             : 
+    1187         623 :   if (takeoff_successful_) {
+    1188             : 
+    1189          57 :     if (got_gps_est || got_rtk_est) {
+    1190             : 
+    1191         114 :       auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+    1192             : 
+    1193         171 :       auto res = transformer_->transformSingle(land_there_reference, "latlon_origin");
+    1194             : 
+    1195          57 :       if (res) {
+    1196          57 :         diag.home_latitude  = res.value().reference.position.x;
+    1197          57 :         diag.home_longitude = res.value().reference.position.y;
+    1198             :       }
+    1199             :     }
+    1200             :   }
+    1201             : 
+    1202         623 :   ph_diag_.publish(diag);
+    1203             : }
+    1204             : 
+    1205             : //}
+    1206             : 
+    1207             : /* //{ timerMidairActivation() */
+    1208             : 
+    1209          33 : void UavManager::timerMidairActivation([[maybe_unused]] const ros::TimerEvent& event) {
+    1210             : 
+    1211          33 :   if (!is_initialized_)
+    1212           0 :     return;
+    1213             : 
+    1214          33 :   ROS_INFO_THROTTLE(0.1, "[UavManager]: waiting for OFFBOARD");
+    1215             : 
+    1216          33 :   if (sh_hw_api_status_.getMsg()->offboard) {
+    1217             : 
+    1218          33 :     ROS_INFO("[UavManager]: OFFBOARD detected");
+    1219             : 
+    1220          33 :     setOdometryCallbacksSrv(true);
+    1221             : 
+    1222             :     {
+    1223          33 :       bool controller_switched = switchControllerSrv(_midair_activation_after_controller_);
+    1224             : 
+    1225          33 :       if (!controller_switched) {
+    1226             : 
+    1227           0 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_controller_.c_str());
+    1228             : 
+    1229           0 :         ehoverSrv();
+    1230             : 
+    1231           0 :         timer_midair_activation_.stop();
+    1232             : 
+    1233           0 :         return;
+    1234             :       }
+    1235             :     }
+    1236             : 
+    1237             :     {
+    1238          33 :       bool tracker_switched = switchTrackerSrv(_midair_activation_after_tracker_);
+    1239             : 
+    1240          33 :       if (!tracker_switched) {
+    1241             : 
+    1242           0 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_tracker_.c_str());
+    1243             : 
+    1244           0 :         ehoverSrv();
+    1245             : 
+    1246           0 :         timer_midair_activation_.stop();
+    1247             : 
+    1248           0 :         return;
+    1249             :       }
+    1250             :     }
+    1251             : 
+    1252          33 :     timer_midair_activation_.stop();
+    1253             : 
+    1254          33 :     return;
+    1255             :   }
+    1256             : 
+    1257           0 :   if ((ros::Time::now() - midair_activation_started_).toSec() > 0.5) {
+    1258             : 
+    1259           0 :     ROS_ERROR("[UavManager]: waiting for OFFBOARD timeouted, reverting");
+    1260             : 
+    1261           0 :     toggleControlOutput(false);
+    1262             : 
+    1263           0 :     timer_midair_activation_.stop();
+    1264             : 
+    1265           0 :     return;
+    1266             :   }
+    1267             : }
+    1268             : 
+    1269             : //}
+    1270             : 
+    1271             : // --------------------------------------------------------------
+    1272             : // |                          callbacks                         |
+    1273             : // --------------------------------------------------------------
+    1274             : 
+    1275             : // | --------------------- topic callbacks -------------------- |
+    1276             : 
+    1277             : /* //{ callbackHwApiGNSS() */
+    1278             : 
+    1279       31133 : void UavManager::callbackHwApiGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    1280             : 
+    1281       31133 :   if (!is_initialized_)
+    1282          18 :     return;
+    1283             : 
+    1284       93345 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiGNSS");
+    1285       93345 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::callbackHwApiGNSS", scope_timer_logger_, scope_timer_enabled_);
+    1286             : 
+    1287       31115 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    1288             : }
+    1289             : 
+    1290             : //}
+    1291             : 
+    1292             : /* //{ callbackOdometry() */
+    1293             : 
+    1294       46391 : void UavManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    1295             : 
+    1296       46391 :   if (!is_initialized_)
+    1297           0 :     return;
+    1298             : 
+    1299       46391 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    1300             : }
+    1301             : 
+    1302             : //}
+    1303             : 
+    1304             : // | -------------------- service callbacks ------------------- |
+    1305             : 
+    1306             : /* //{ callbackTakeoff() */
+    1307             : 
+    1308           6 : bool UavManager::callbackTakeoff([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1309             : 
+    1310           6 :   if (!is_initialized_)
+    1311           0 :     return false;
+    1312             : 
+    1313           6 :   ROS_INFO("[UavManager]: takeoff called by service");
+    1314             : 
+    1315             :   /* preconditions //{ */
+    1316             : 
+    1317             :   {
+    1318           6 :     std::stringstream ss;
+    1319             : 
+    1320           6 :     if (!sh_odometry_.hasMsg()) {
+    1321           0 :       ss << "can not takeoff, missing odometry!";
+    1322           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1323           0 :       res.message = ss.str();
+    1324           0 :       res.success = false;
+    1325           0 :       return true;
+    1326             :     }
+    1327             : 
+    1328           6 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1329           0 :       ss << "can not takeoff, missing HW API status!";
+    1330           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1331           0 :       res.message = ss.str();
+    1332           0 :       res.success = false;
+    1333           0 :       return true;
+    1334             :     }
+    1335             : 
+    1336           6 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1337           0 :       ss << "can not takeoff, UAV not armed!";
+    1338           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1339           0 :       res.message = ss.str();
+    1340           0 :       res.success = false;
+    1341           0 :       return true;
+    1342             :     }
+    1343             : 
+    1344           6 :     if (!sh_hw_api_status_.getMsg()->offboard) {
+    1345           0 :       ss << "can not takeoff, UAV not in offboard mode!";
+    1346           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1347           0 :       res.message = ss.str();
+    1348           0 :       res.success = false;
+    1349           0 :       return true;
+    1350             :     }
+    1351             : 
+    1352             :     {
+    1353           6 :       if (!sh_control_manager_diag_.hasMsg() && (ros::Time::now() - sh_control_manager_diag_.lastMsgTime()).toSec() > 5.0) {
+    1354           0 :         ss << "can not takeoff, missing control manager diagnostics!";
+    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           6 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1362           0 :         ss << "can not takeoff, need '" << _null_tracker_name_ << "' to be active!";
+    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           6 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1371           0 :       ss << "can not takeoff, missing controller 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           6 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1379           0 :       ss << "can not takeoff, GainManager is not running!";
+    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           6 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    1387           0 :       ss << "can not takeoff, ConstraintManager is not running!";
+    1388           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1389           0 :       res.message = ss.str();
+    1390           0 :       res.success = false;
+    1391           0 :       return true;
+    1392             :     }
+    1393             : 
+    1394           6 :     if (!sh_control_manager_diag_.getMsg()->output_enabled) {
+    1395             : 
+    1396           0 :       ss << "can not takeoff, Control Manager's output is disabled!";
+    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           6 :     if (number_of_takeoffs_ > 0) {
+    1404             : 
+    1405           0 :       auto last_mass_difference = mrs_lib::get_mutexed(mutex_last_mass_difference_, last_mass_difference_);
+    1406             : 
+    1407           0 :       if (last_mass_difference > 1.0) {
+    1408             : 
+    1409           0 :         ss << std::setprecision(2);
+    1410           0 :         ss << "can not takeoff, estimated mass difference is too large: " << _null_tracker_name_ << "!";
+    1411           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1412           0 :         res.message = ss.str();
+    1413           0 :         res.success = false;
+    1414           0 :         return true;
+    1415             :       }
+    1416             :     }
+    1417             :   }
+    1418             : 
+    1419             :   //}
+    1420             : 
+    1421          12 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+    1422          12 :   auto odometry                    = sh_odometry_.getMsg();
+    1423           6 :   auto [odom_x, odom_y, odom_z]    = mrs_lib::getPosition(sh_odometry_.getMsg());
+    1424             : 
+    1425             :   double odom_heading;
+    1426             :   try {
+    1427           6 :     odom_heading = mrs_lib::getHeading(sh_odometry_.getMsg());
+    1428             :   }
+    1429           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    1430           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+    1431             : 
+    1432           0 :     std::stringstream ss;
+    1433           0 :     ss << "could not calculate current heading";
+    1434           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1435           0 :     res.message = ss.str();
+    1436           0 :     res.success = false;
+    1437           0 :     return true;
+    1438             :   }
+    1439             : 
+    1440           6 :   ROS_INFO("[UavManager]: taking off");
+    1441             : 
+    1442           6 :   setOdometryCallbacksSrv(false);
+    1443             : 
+    1444             :   // activating the takeoff controller
+    1445             :   {
+    1446           6 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    1447           6 :     bool        controller_switched = switchControllerSrv(_takeoff_controller_name_);
+    1448             : 
+    1449             :     // if it fails, activate back the old controller
+    1450             :     // this is no big deal since the control outputs are not used
+    1451             :     // until NullTracker is active
+    1452           6 :     if (!controller_switched) {
+    1453             : 
+    1454           0 :       std::stringstream ss;
+    1455           0 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for takeoff";
+    1456           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1457           0 :       res.success = false;
+    1458           0 :       res.message = ss.str();
+    1459             : 
+    1460           0 :       switchControllerSrv(old_controller);
+    1461             : 
+    1462           0 :       return true;
+    1463             :     }
+    1464             :   }
+    1465             : 
+    1466             :   // activate the takeoff tracker
+    1467             :   {
+    1468           6 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    1469           6 :     bool        tracker_switched = switchTrackerSrv(_takeoff_tracker_name_);
+    1470             : 
+    1471             :     // if it fails, activate back the old tracker
+    1472           6 :     if (!tracker_switched) {
+    1473             : 
+    1474           0 :       std::stringstream ss;
+    1475           0 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for takeoff";
+    1476           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1477           0 :       res.success = false;
+    1478           0 :       res.message = ss.str();
+    1479             : 
+    1480           0 :       switchTrackerSrv(old_tracker);
+    1481             : 
+    1482           0 :       return true;
+    1483             :     }
+    1484             :   }
+    1485             : 
+    1486             :   // let's sleep before calling take off.. the motors are rumping-up anyway
+    1487             :   // this solves on-time-happening race condition in the landoff tracker
+    1488           6 :   ros::Duration(0.3).sleep();
+    1489             : 
+    1490             :   // now the takeoff tracker and controller are active
+    1491             :   // the UAV is basically hovering on the ground
+    1492             :   // (the controller is probably rumping up the throttle now)
+    1493             : 
+    1494             :   // call the takeoff service at the takeoff tracker
+    1495             :   {
+    1496           6 :     bool takeoff_successful = takeoffSrv();
+    1497             : 
+    1498             :     // if the takeoff was not successful, switch to NullTracker
+    1499           6 :     if (takeoff_successful) {
+    1500             : 
+    1501             :       // save the current spot for later landing
+    1502             :       {
+    1503           6 :         std::scoped_lock lock(mutex_land_there_reference_);
+    1504             : 
+    1505           6 :         land_there_reference_.header               = odometry->header;
+    1506           6 :         land_there_reference_.reference.position.x = odom_x;
+    1507           6 :         land_there_reference_.reference.position.y = odom_y;
+    1508           6 :         land_there_reference_.reference.position.z = odom_z;
+    1509           6 :         land_there_reference_.reference.heading    = odom_heading;
+    1510             :       }
+    1511             : 
+    1512           6 :       timer_flighttime_.start();
+    1513             : 
+    1514          12 :       std::stringstream ss;
+    1515           6 :       ss << "taking off";
+    1516           6 :       res.success = true;
+    1517           6 :       res.message = ss.str();
+    1518           6 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1519             : 
+    1520           6 :       takingoff_ = true;
+    1521           6 :       number_of_takeoffs_++;
+    1522           6 :       waiting_for_takeoff_ = true;
+    1523             : 
+    1524             :       // start the takeoff timer
+    1525           6 :       timer_takeoff_.start();
+    1526             : 
+    1527           6 :       takeoff_successful_ = takeoff_successful;
+    1528             : 
+    1529             :     } else {
+    1530             : 
+    1531           0 :       std::stringstream ss;
+    1532           0 :       ss << "takeoff was not successful";
+    1533           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1534           0 :       res.success = false;
+    1535           0 :       res.message = ss.str();
+    1536             : 
+    1537             :       // if the call for takeoff fails, call for emergency landing
+    1538           0 :       elandSrv();
+    1539             :     }
+    1540             :   }
+    1541             : 
+    1542           6 :   return true;
+    1543             : }
+    1544             : 
+    1545             : //}
+    1546             : 
+    1547             : /* //{ callbackLand() */
+    1548             : 
+    1549           2 : bool UavManager::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1550             : 
+    1551           2 :   if (!is_initialized_)
+    1552           0 :     return false;
+    1553             : 
+    1554           2 :   ROS_INFO("[UavManager]: land called by service");
+    1555             : 
+    1556             :   /* preconditions //{ */
+    1557             : 
+    1558             :   {
+    1559           2 :     std::stringstream ss;
+    1560             : 
+    1561           2 :     if (!sh_odometry_.hasMsg()) {
+    1562           0 :       ss << "can not land, missing odometry!";
+    1563           0 :       res.message = ss.str();
+    1564           0 :       res.success = false;
+    1565           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1566           0 :       return true;
+    1567             :     }
+    1568             : 
+    1569             :     {
+    1570           2 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1571           0 :         ss << "can not land, missing control manager diagnostics!";
+    1572           0 :         res.message = ss.str();
+    1573           0 :         res.success = false;
+    1574           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1575           0 :         return true;
+    1576             :       }
+    1577             : 
+    1578           2 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1579           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1580           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1581           0 :         res.message = ss.str();
+    1582           0 :         res.success = false;
+    1583           0 :         return true;
+    1584             :       }
+    1585             :     }
+    1586             : 
+    1587           2 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1588           0 :       ss << "can not land, missing controller diagnostics!";
+    1589           0 :       res.message = ss.str();
+    1590           0 :       res.success = false;
+    1591           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1592           0 :       return true;
+    1593             :     }
+    1594             : 
+    1595           2 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1596           0 :       ss << "can not land, missing position cmd!";
+    1597           0 :       res.message = ss.str();
+    1598           0 :       res.success = false;
+    1599           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1600           0 :       return true;
+    1601             :     }
+    1602             :   }
+    1603             : 
+    1604             :   //}
+    1605             : 
+    1606           2 :   auto [success, message] = landWithDescendImpl();
+    1607             : 
+    1608           2 :   res.message = message;
+    1609           2 :   res.success = success;
+    1610             : 
+    1611           2 :   return true;
+    1612             : }
+    1613             : 
+    1614             : //}
+    1615             : 
+    1616             : /* //{ callbackLandHome() */
+    1617             : 
+    1618           0 : bool UavManager::callbackLandHome([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1619             : 
+    1620           0 :   if (!is_initialized_)
+    1621           0 :     return false;
+    1622             : 
+    1623           0 :   ROS_INFO("[UavManager]: land home called by service");
+    1624             : 
+    1625             :   /* preconditions //{ */
+    1626             : 
+    1627             :   {
+    1628           0 :     std::stringstream ss;
+    1629             : 
+    1630           0 :     if (number_of_takeoffs_ == 0) {
+    1631           0 :       ss << "can not land home, did not takeoff before!";
+    1632           0 :       res.message = ss.str();
+    1633           0 :       res.success = false;
+    1634           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1635           0 :       return true;
+    1636             :     }
+    1637             : 
+    1638           0 :     if (!sh_odometry_.hasMsg()) {
+    1639           0 :       ss << "can not land, missing odometry!";
+    1640           0 :       res.message = ss.str();
+    1641           0 :       res.success = false;
+    1642           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1643           0 :       return true;
+    1644             :     }
+    1645             : 
+    1646             :     {
+    1647           0 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1648           0 :         ss << "can not land, missing tracker status!";
+    1649           0 :         res.message = ss.str();
+    1650           0 :         res.success = false;
+    1651           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1652           0 :         return true;
+    1653             :       }
+    1654             : 
+    1655           0 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1656           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1657           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1658           0 :         res.message = ss.str();
+    1659           0 :         res.success = false;
+    1660           0 :         return true;
+    1661             :       }
+    1662             :     }
+    1663             : 
+    1664           0 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1665           0 :       ss << "can not land, missing controller diagnostics command!";
+    1666           0 :       res.message = ss.str();
+    1667           0 :       res.success = false;
+    1668           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1669           0 :       return true;
+    1670             :     }
+    1671             : 
+    1672           0 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1673           0 :       ss << "can not land, missing position cmd!";
+    1674           0 :       res.message = ss.str();
+    1675           0 :       res.success = false;
+    1676           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1677           0 :       return true;
+    1678             :     }
+    1679             : 
+    1680           0 :     if (fixing_max_height_) {
+    1681           0 :       ss << "can not land, descedning to safe height!";
+    1682           0 :       res.message = ss.str();
+    1683           0 :       res.success = false;
+    1684           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1685           0 :       return true;
+    1686             :     }
+    1687             : 
+    1688           0 :     if (current_state_landing_ != IDLE_STATE) {
+    1689           0 :       ss << "can not land, already landing!";
+    1690           0 :       res.message = ss.str();
+    1691           0 :       res.success = false;
+    1692           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1693           0 :       return true;
+    1694             :     }
+    1695             :   }
+    1696             : 
+    1697             :   //}
+    1698             : 
+    1699           0 :   ungripSrv();
+    1700             : 
+    1701           0 :   mrs_msgs::ReferenceStamped reference_out;
+    1702             : 
+    1703             :   {
+    1704           0 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1705             : 
+    1706             :     // get the current altitude in land_there_reference_.header.frame_id;
+    1707           0 :     geometry_msgs::PoseStamped current_pose;
+    1708           0 :     current_pose.header.stamp     = ros::Time::now();
+    1709           0 :     current_pose.header.frame_id  = _uav_name_ + "/fcu";
+    1710           0 :     current_pose.pose.position.x  = 0;
+    1711           0 :     current_pose.pose.position.y  = 0;
+    1712           0 :     current_pose.pose.position.z  = 0;
+    1713           0 :     current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1714             : 
+    1715           0 :     auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+    1716             : 
+    1717           0 :     if (response) {
+    1718             : 
+    1719           0 :       land_there_reference_.reference.position.z = response.value().pose.position.z;
+    1720           0 :       ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+    1721             : 
+    1722             :     } else {
+    1723             : 
+    1724           0 :       std::stringstream ss;
+    1725           0 :       ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+    1726           0 :       ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1727             : 
+    1728           0 :       res.success = false;
+    1729           0 :       res.message = ss.str();
+    1730           0 :       return true;
+    1731             :     }
+    1732             : 
+    1733           0 :     reference_out.header.frame_id = land_there_reference_.header.frame_id;
+    1734           0 :     reference_out.header.stamp    = ros::Time::now();
+    1735           0 :     reference_out.reference       = land_there_reference_.reference;
+    1736             :   }
+    1737             : 
+    1738           0 :   bool service_success = emergencyReferenceSrv(reference_out);
+    1739             : 
+    1740           0 :   if (service_success) {
+    1741             : 
+    1742           0 :     std::stringstream ss;
+    1743           0 :     ss << "flying home for landing";
+    1744           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1745             : 
+    1746           0 :     res.success = true;
+    1747           0 :     res.message = ss.str();
+    1748             : 
+    1749             :     // stop the eventual takeoff
+    1750           0 :     waiting_for_takeoff_ = false;
+    1751           0 :     takingoff_           = false;
+    1752           0 :     timer_takeoff_.stop();
+    1753             : 
+    1754           0 :     throttle_under_threshold_          = false;
+    1755           0 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1756             : 
+    1757           0 :     changeLandingState(FLY_THERE_STATE);
+    1758             : 
+    1759           0 :     timer_landing_.start();
+    1760             : 
+    1761             :   } else {
+    1762             : 
+    1763           0 :     std::stringstream ss;
+    1764           0 :     ss << "can not fly home for landing";
+    1765           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1766             : 
+    1767           0 :     res.success = false;
+    1768           0 :     res.message = ss.str();
+    1769             :   }
+    1770             : 
+    1771           0 :   return true;
+    1772             : }
+    1773             : 
+    1774             : //}
+    1775             : 
+    1776             : /* //{ callbackLandThere() */
+    1777             : 
+    1778           0 : bool UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    1779             : 
+    1780           0 :   if (!is_initialized_)
+    1781           0 :     return false;
+    1782             : 
+    1783           0 :   ROS_INFO("[UavManager]: land there called by service");
+    1784             : 
+    1785             :   /* preconditions //{ */
+    1786             : 
+    1787             :   {
+    1788           0 :     std::stringstream ss;
+    1789             : 
+    1790           0 :     if (!sh_odometry_.hasMsg()) {
+    1791           0 :       ss << "can not land, missing odometry!";
+    1792           0 :       res.message = ss.str();
+    1793           0 :       res.success = false;
+    1794           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1795           0 :       return true;
+    1796             :     }
+    1797             : 
+    1798             :     {
+    1799           0 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1800           0 :         ss << "can not land, missing tracker status!";
+    1801           0 :         res.message = ss.str();
+    1802           0 :         res.success = false;
+    1803           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1804           0 :         return true;
+    1805             :       }
+    1806             : 
+    1807           0 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1808           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1809           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1810           0 :         res.message = ss.str();
+    1811           0 :         res.success = false;
+    1812           0 :         return true;
+    1813             :       }
+    1814             :     }
+    1815             : 
+    1816           0 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1817           0 :       ss << "can not land, missing controller diagnostics!";
+    1818           0 :       res.message = ss.str();
+    1819           0 :       res.success = false;
+    1820           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1821           0 :       return true;
+    1822             :     }
+    1823             : 
+    1824           0 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1825           0 :       ss << "can not land, missing position cmd!";
+    1826           0 :       res.message = ss.str();
+    1827           0 :       res.success = false;
+    1828           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1829           0 :       return true;
+    1830             :     }
+    1831             : 
+    1832           0 :     if (fixing_max_height_) {
+    1833           0 :       ss << "can not land, descedning to safe height!";
+    1834           0 :       res.message = ss.str();
+    1835           0 :       res.success = false;
+    1836           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1837           0 :       return true;
+    1838             :     }
+    1839             :   }
+    1840             : 
+    1841             :   //}
+    1842             : 
+    1843           0 :   ungripSrv();
+    1844             : 
+    1845           0 :   mrs_msgs::ReferenceStamped reference_out;
+    1846             : 
+    1847             :   {
+    1848           0 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1849             : 
+    1850           0 :     land_there_reference_.header    = req.header;
+    1851           0 :     land_there_reference_.reference = req.reference;
+    1852             : 
+    1853           0 :     reference_out.header.frame_id = land_there_reference_.header.frame_id;
+    1854           0 :     reference_out.header.stamp    = ros::Time::now();
+    1855           0 :     reference_out.reference       = land_there_reference_.reference;
+    1856             :   }
+    1857             : 
+    1858           0 :   bool service_success = emergencyReferenceSrv(reference_out);
+    1859             : 
+    1860           0 :   if (service_success) {
+    1861             : 
+    1862           0 :     std::stringstream ss;
+    1863           0 :     ss << "flying there for landing";
+    1864           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1865             : 
+    1866           0 :     res.success = true;
+    1867           0 :     res.message = ss.str();
+    1868             : 
+    1869             :     // stop the eventual takeoff
+    1870           0 :     waiting_for_takeoff_ = false;
+    1871           0 :     takingoff_           = false;
+    1872           0 :     timer_takeoff_.stop();
+    1873             : 
+    1874           0 :     throttle_under_threshold_          = false;
+    1875           0 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1876             : 
+    1877           0 :     changeLandingState(FLY_THERE_STATE);
+    1878             : 
+    1879           0 :     timer_landing_.start();
+    1880             : 
+    1881             :   } else {
+    1882             : 
+    1883           0 :     std::stringstream ss;
+    1884           0 :     ss << "can not fly there for landing";
+    1885           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1886             : 
+    1887           0 :     res.success = false;
+    1888           0 :     res.message = ss.str();
+    1889             :   }
+    1890             : 
+    1891           0 :   return true;
+    1892             : }
+    1893             : 
+    1894             : //}
+    1895             : 
+    1896             : /* //{ callbackMidairActivation() */
+    1897             : 
+    1898          33 : bool UavManager::callbackMidairActivation([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1899             : 
+    1900          33 :   if (!is_initialized_)
+    1901           0 :     return false;
+    1902             : 
+    1903          33 :   ROS_INFO("[UavManager]: midair activation called by service");
+    1904             : 
+    1905             :   /* preconditions //{ */
+    1906             : 
+    1907             :   {
+    1908          33 :     std::stringstream ss;
+    1909             : 
+    1910          33 :     if (!sh_odometry_.hasMsg()) {
+    1911           0 :       ss << "can not activate, missing odometry!";
+    1912           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1913           0 :       res.message = ss.str();
+    1914           0 :       res.success = false;
+    1915           0 :       return true;
+    1916             :     }
+    1917             : 
+    1918          33 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1919           0 :       ss << "can not activate, missing HW API status!";
+    1920           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1921           0 :       res.message = ss.str();
+    1922           0 :       res.success = false;
+    1923           0 :       return true;
+    1924             :     }
+    1925             : 
+    1926          33 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1927           0 :       ss << "can not activate, UAV not armed!";
+    1928           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1929           0 :       res.message = ss.str();
+    1930           0 :       res.success = false;
+    1931           0 :       return true;
+    1932             :     }
+    1933             : 
+    1934          33 :     if (sh_hw_api_status_.getMsg()->offboard) {
+    1935           0 :       ss << "can not activate, UAV already in offboard mode!";
+    1936           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1937           0 :       res.message = ss.str();
+    1938           0 :       res.success = false;
+    1939           0 :       return true;
+    1940             :     }
+    1941             : 
+    1942             :     {
+    1943          33 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1944           0 :         ss << "can not activate, missing control manager diagnostics!";
+    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          33 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1952           0 :         ss << "can not activate, need '" << _null_tracker_name_ << "' to be active!";
+    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             : 
+    1960          33 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1961           0 :       ss << "can not activate, missing controller diagnostics!";
+    1962           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1963           0 :       res.message = ss.str();
+    1964           0 :       res.success = false;
+    1965           0 :       return true;
+    1966             :     }
+    1967             : 
+    1968          33 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1969           0 :       ss << "can not activate, GainManager is not running!";
+    1970           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1971           0 :       res.message = ss.str();
+    1972           0 :       res.success = false;
+    1973           0 :       return true;
+    1974             :     }
+    1975             : 
+    1976          33 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    1977           0 :       ss << "can not activate, ConstraintManager is not running!";
+    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          33 :     if (number_of_takeoffs_ > 0) {
+    1985           0 :       ss << "can not activate, we flew already!";
+    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             :   //}
+    1994             : 
+    1995          33 :   auto [success, message] = midairActivationImpl();
+    1996             : 
+    1997          33 :   res.message = message;
+    1998          33 :   res.success = success;
+    1999             : 
+    2000          33 :   return true;
+    2001             : }
+    2002             : 
+    2003             : //}
+    2004             : 
+    2005             : // | ------------------------ routines ------------------------ |
+    2006             : 
+    2007             : /* landImpl() //{ */
+    2008             : 
+    2009           2 : std::tuple<bool, std::string> UavManager::landImpl(void) {
+    2010             : 
+    2011             :   // activating the landing controller
+    2012             :   {
+    2013           2 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    2014           2 :     bool        controller_switched = switchControllerSrv(_landing_controller_name_);
+    2015             : 
+    2016             :     // if it fails, activate eland
+    2017             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2018             :     // just throw out error.
+    2019           2 :     if (!controller_switched) {
+    2020             : 
+    2021           0 :       std::stringstream ss;
+    2022           0 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for landing";
+    2023           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2024             : 
+    2025           0 :       elandSrv();
+    2026             : 
+    2027           0 :       return std::tuple(false, ss.str());
+    2028             :     }
+    2029             :   }
+    2030             : 
+    2031             :   // activate the landing tracker
+    2032             :   {
+    2033           2 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    2034           2 :     bool        tracker_switched = switchTrackerSrv(_landing_tracker_name_);
+    2035             : 
+    2036             :     // if it fails, activate eland
+    2037             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2038             :     // just throw out error.
+    2039           2 :     if (!tracker_switched) {
+    2040             : 
+    2041           0 :       std::stringstream ss;
+    2042           0 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for landing";
+    2043           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2044             : 
+    2045           0 :       elandSrv();
+    2046             : 
+    2047           0 :       return std::tuple(false, ss.str());
+    2048             :     }
+    2049             :   }
+    2050             : 
+    2051             :   // call the landing service
+    2052             :   {
+    2053           2 :     bool land_successful = landSrv();
+    2054             : 
+    2055           2 :     if (land_successful) {
+    2056             : 
+    2057             :       // stop the eventual takeoff
+    2058           2 :       waiting_for_takeoff_ = false;
+    2059           2 :       takingoff_           = false;
+    2060           2 :       timer_takeoff_.stop();
+    2061             : 
+    2062             :       // stop counting the flight time
+    2063           2 :       timer_flighttime_.stop();
+    2064             : 
+    2065           4 :       auto controller_diagnostics = sh_controller_diagnostics_.getMsg();
+    2066             : 
+    2067             :       // remember the last valid mass estimated
+    2068             :       // used during subsequent takeoff
+    2069           2 :       if (controller_diagnostics->mass_estimator) {
+    2070           2 :         last_mass_difference_ = controller_diagnostics->mass_difference;
+    2071             :       }
+    2072             : 
+    2073           2 :       setOdometryCallbacksSrv(false);
+    2074             : 
+    2075           2 :       changeLandingState(LANDING_STATE);
+    2076             : 
+    2077           2 :       throttle_under_threshold_          = false;
+    2078           2 :       throttle_mass_estimate_first_time_ = ros::Time(0);
+    2079             : 
+    2080           2 :       timer_landing_.start();
+    2081             : 
+    2082           2 :       std::stringstream ss;
+    2083           2 :       ss << "landing initiated";
+    2084           2 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2085             : 
+    2086           2 :       return std::tuple(true, ss.str());
+    2087             : 
+    2088             :     } else {
+    2089             : 
+    2090           0 :       std::stringstream ss;
+    2091           0 :       ss << "could not land";
+    2092           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2093             : 
+    2094           0 :       elandSrv();
+    2095             : 
+    2096           0 :       return std::tuple(false, ss.str());
+    2097             :     }
+    2098             :   }
+    2099             : }
+    2100             : 
+    2101             : //}
+    2102             : 
+    2103             : /* landWithDescendImpl() //{ */
+    2104             : 
+    2105           2 : std::tuple<bool, std::string> UavManager::landWithDescendImpl(void) {
+    2106             : 
+    2107             :   // if the height information is available
+    2108           2 :   if (sh_height_.hasMsg()) {
+    2109             : 
+    2110           2 :     double height = sh_height_.getMsg()->value;
+    2111             : 
+    2112           2 :     if (height > 0 && height >= _landing_descend_height_ + 1.0) {
+    2113             : 
+    2114           0 :       auto odometry = sh_odometry_.getMsg();
+    2115             : 
+    2116           0 :       ungripSrv();
+    2117             : 
+    2118             :       {
+    2119           0 :         std::scoped_lock lock(mutex_land_there_reference_);
+    2120             : 
+    2121             :         // FOR FUTURE ME: Do not change this, we need it to be filled for the final check later
+    2122           0 :         land_there_reference_.header.frame_id      = "";
+    2123           0 :         land_there_reference_.header.stamp         = ros::Time::now();
+    2124           0 :         land_there_reference_.reference.position.x = odometry->pose.pose.position.x;
+    2125           0 :         land_there_reference_.reference.position.y = odometry->pose.pose.position.y;
+    2126           0 :         land_there_reference_.reference.position.z = odometry->pose.pose.position.z - (height - _landing_descend_height_);
+    2127           0 :         land_there_reference_.reference.heading    = mrs_lib::AttitudeConverter(odometry->pose.pose.orientation).getHeading();
+    2128             :       }
+    2129             : 
+    2130           0 :       bool service_success = emergencyReferenceSrv(land_there_reference_);
+    2131             : 
+    2132           0 :       if (service_success) {
+    2133             : 
+    2134           0 :         std::stringstream ss;
+    2135           0 :         ss << "flying down for landing";
+    2136           0 :         ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2137             : 
+    2138             :         // stop the eventual takeoff
+    2139           0 :         waiting_for_takeoff_ = false;
+    2140           0 :         takingoff_           = false;
+    2141           0 :         timer_takeoff_.stop();
+    2142             : 
+    2143           0 :         changeLandingState(FLY_THERE_STATE);
+    2144             : 
+    2145           0 :         throttle_under_threshold_          = false;
+    2146           0 :         throttle_mass_estimate_first_time_ = ros::Time(0);
+    2147             : 
+    2148           0 :         timer_landing_.start();
+    2149             : 
+    2150           0 :         return std::tuple(true, ss.str());
+    2151             : 
+    2152             :       } else {
+    2153             : 
+    2154           0 :         std::stringstream ss;
+    2155           0 :         ss << "can not fly down for landing";
+    2156           0 :         ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    2157             :       }
+    2158             :     }
+    2159             :   }
+    2160             : 
+    2161           4 :   auto [success, message] = landImpl();
+    2162             : 
+    2163           2 :   return std::tuple(success, message);
+    2164             : }
+    2165             : 
+    2166             : //}
+    2167             : 
+    2168             : /* midairActivationImpl() //{ */
+    2169             : 
+    2170          33 : std::tuple<bool, std::string> UavManager::midairActivationImpl(void) {
+    2171             : 
+    2172             :   // 1. activate the mid-air activation controller
+    2173             :   // the controller will output hover-like control output
+    2174          66 :   std::string old_controller;
+    2175             :   {
+    2176          33 :     old_controller           = sh_control_manager_diag_.getMsg()->active_controller;
+    2177          33 :     bool controller_switched = switchControllerSrv(_midair_activation_during_controller_);
+    2178             : 
+    2179          33 :     if (!controller_switched) {
+    2180             : 
+    2181           0 :       std::stringstream ss;
+    2182           0 :       ss << "could not activate '" << _midair_activation_during_controller_ << "' for midair activation";
+    2183           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2184             : 
+    2185           0 :       return std::tuple(false, ss.str());
+    2186             :     }
+    2187             :   }
+    2188             : 
+    2189             :   // 2. turn Control Manager's output ON
+    2190             :   {
+    2191          33 :     bool output_enabled = toggleControlOutput(true);
+    2192             : 
+    2193          33 :     if (!output_enabled) {
+    2194             : 
+    2195           0 :       switchControllerSrv(old_controller);
+    2196             : 
+    2197           0 :       std::stringstream ss;
+    2198           0 :       ss << "could not enable Control Manager's output";
+    2199           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2200             : 
+    2201           0 :       return std::tuple(false, ss.str());
+    2202             :     }
+    2203             :   }
+    2204             : 
+    2205             :   // 3. activate the mid-air activation tracker
+    2206             :   // this will cause the Control Manager to output something else than min-throttle
+    2207          66 :   std::string old_tracker;
+    2208             :   {
+    2209          33 :     old_tracker = sh_control_manager_diag_.getMsg()->active_tracker;
+    2210             : 
+    2211          33 :     bool tracker_switched = switchTrackerSrv(_midair_activation_during_tracker_);
+    2212             : 
+    2213          33 :     if (!tracker_switched) {
+    2214             : 
+    2215           0 :       switchControllerSrv(old_controller);
+    2216           0 :       toggleControlOutput(false);
+    2217             : 
+    2218           0 :       std::stringstream ss;
+    2219           0 :       ss << "could not activate '" << _midair_activation_during_tracker_ << "' for midair activation";
+    2220           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2221             : 
+    2222           0 :       return std::tuple(false, ss.str());
+    2223             :     }
+    2224             :   }
+    2225             : 
+    2226             :   // 4. wait for 50 ms, that should be enough for the Pixhawk to start getting data
+    2227          33 :   ros::Duration(0.05).sleep();
+    2228             : 
+    2229             :   // 5. turn on the OFFBOARD MODE
+    2230             :   // since now, the UAV should be under our control
+    2231             :   {
+    2232          33 :     bool offboard_set = offboardSrv(true);
+    2233             : 
+    2234          33 :     if (!offboard_set) {
+    2235             : 
+    2236           0 :       switchTrackerSrv(old_tracker);
+    2237           0 :       switchControllerSrv(old_controller);
+    2238           0 :       toggleControlOutput(false);
+    2239             : 
+    2240           0 :       std::stringstream ss;
+    2241           0 :       ss << "could not activate offboard mode";
+    2242           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2243             : 
+    2244           0 :       return std::tuple(false, ss.str());
+    2245             :     }
+    2246             :   }
+    2247             : 
+    2248             :   // remember this time, later check for timeout
+    2249          33 :   midair_activation_started_ = ros::Time::now();
+    2250             : 
+    2251             :   // start the timer which should check if the offboard is on, activate proper controller and tracker or timeout
+    2252          33 :   timer_midair_activation_.start();
+    2253             : 
+    2254          33 :   std::stringstream ss;
+    2255          33 :   ss << "midair activation initiated, starting the timer";
+    2256          33 :   ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2257             : 
+    2258          33 :   return std::tuple(true, ss.str());
+    2259             : }
+    2260             : 
+    2261             : //}
+    2262             : 
+    2263             : // | ----------------- service client wrappers ---------------- |
+    2264             : 
+    2265             : /* setOdometryCallbacksSrv() //{ */
+    2266             : 
+    2267          47 : void UavManager::setOdometryCallbacksSrv(const bool& input) {
+    2268             : 
+    2269          47 :   ROS_INFO("[UavManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    2270             : 
+    2271          94 :   std_srvs::SetBool srv;
+    2272             : 
+    2273          47 :   srv.request.data = input;
+    2274             : 
+    2275          47 :   bool res = sch_odometry_callbacks_.call(srv);
+    2276             : 
+    2277          47 :   if (res) {
+    2278             : 
+    2279          47 :     if (!srv.response.success) {
+    2280           0 :       ROS_WARN("[UavManager]: service call for toggle odometry callbacks returned: %s.", srv.response.message.c_str());
+    2281             :     }
+    2282             : 
+    2283             :   } else {
+    2284           0 :     ROS_ERROR("[UavManager]: service call for toggle odometry callbacks failed!");
+    2285             :   }
+    2286          47 : }
+    2287             : 
+    2288             : //}
+    2289             : 
+    2290             : /* setControlCallbacksSrv() //{ */
+    2291             : 
+    2292           2 : void UavManager::setControlCallbacksSrv(const bool& input) {
+    2293             : 
+    2294           2 :   ROS_INFO("[UavManager]: switching control callbacks to %s", input ? "ON" : "OFF");
+    2295             : 
+    2296           4 :   std_srvs::SetBool srv;
+    2297             : 
+    2298           2 :   srv.request.data = input;
+    2299             : 
+    2300           2 :   bool res = sch_control_callbacks_.call(srv);
+    2301             : 
+    2302           2 :   if (res) {
+    2303             : 
+    2304           2 :     if (!srv.response.success) {
+    2305           0 :       ROS_WARN("[UavManager]: service call for setting control callbacks returned: %s.", srv.response.message.c_str());
+    2306             :     }
+    2307             : 
+    2308             :   } else {
+    2309           0 :     ROS_ERROR("[UavManager]: service call for setting control callbacks failed!");
+    2310             :   }
+    2311           2 : }
+    2312             : 
+    2313             : //}
+    2314             : 
+    2315             : /* ungripSrv() //{ */
+    2316             : 
+    2317           0 : void UavManager::ungripSrv(void) {
+    2318             : 
+    2319           0 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: ungripping payload");
+    2320             : 
+    2321           0 :   std_srvs::Trigger srv;
+    2322             : 
+    2323           0 :   bool res = sch_ungrip_.call(srv);
+    2324             : 
+    2325           0 :   if (res) {
+    2326             : 
+    2327           0 :     if (!srv.response.success) {
+    2328           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload returned: %s.", srv.response.message.c_str());
+    2329             :     }
+    2330             : 
+    2331             :   } else {
+    2332           0 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload failed!");
+    2333             :   }
+    2334           0 : }
+    2335             : 
+    2336             : //}
+    2337             : 
+    2338             : /* toggleControlOutput() //{ */
+    2339             : 
+    2340          33 : bool UavManager::toggleControlOutput(const bool& input) {
+    2341             : 
+    2342          33 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: toggling control output %s", input ? "ON" : "OFF");
+    2343             : 
+    2344          66 :   std_srvs::SetBool srv;
+    2345             : 
+    2346          33 :   srv.request.data = input;
+    2347             : 
+    2348          33 :   bool res = sch_toggle_control_output_.call(srv);
+    2349             : 
+    2350          33 :   if (res) {
+    2351             : 
+    2352          33 :     if (!srv.response.success) {
+    2353           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output returned: %s.", srv.response.message.c_str());
+    2354           0 :       return false;
+    2355             :     } else {
+    2356          33 :       return true;
+    2357             :     }
+    2358             : 
+    2359             :   } else {
+    2360           0 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output failed!");
+    2361           0 :     return false;
+    2362             :   }
+    2363             : }
+    2364             : 
+    2365             : //}
+    2366             : 
+    2367             : /* offboardSrv() //{ */
+    2368             : 
+    2369          33 : bool UavManager::offboardSrv(const bool in) {
+    2370             : 
+    2371          33 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: setting offboard to %d", in);
+    2372             : 
+    2373          66 :   std_srvs::Trigger srv;
+    2374             : 
+    2375          33 :   bool res = sch_offboard_.call(srv);
+    2376             : 
+    2377          33 :   if (!res) {
+    2378             : 
+    2379           0 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed!");
+    2380           0 :     return false;
+    2381             : 
+    2382             :   } else {
+    2383             : 
+    2384          33 :     if (!srv.response.success) {
+    2385           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed, returned: %s", srv.response.message.c_str());
+    2386           0 :       return false;
+    2387             :     } else {
+    2388          33 :       return true;
+    2389             :     }
+    2390             :   }
+    2391             : }
+    2392             : 
+    2393             : //}
+    2394             : 
+    2395             : /* pirouetteSrv() //{ */
+    2396             : 
+    2397           0 : void UavManager::pirouetteSrv(void) {
+    2398             : 
+    2399           0 :   std_srvs::Trigger srv;
+    2400             : 
+    2401           0 :   bool res = sch_pirouette_.call(srv);
+    2402             : 
+    2403           0 :   if (res) {
+    2404             : 
+    2405           0 :     if (!srv.response.success) {
+    2406           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call for pirouette returned: %s.", srv.response.message.c_str());
+    2407             :     }
+    2408             : 
+    2409             :   } else {
+    2410           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for pirouette failed!");
+    2411             :   }
+    2412           0 : }
+    2413             : 
+    2414             : //}
+    2415             : 
+    2416             : /* disarmSrv() //{ */
+    2417             : 
+    2418           2 : void UavManager::disarmSrv(void) {
+    2419             : 
+    2420           4 :   std_srvs::SetBool srv;
+    2421             : 
+    2422           2 :   srv.request.data = false;
+    2423             : 
+    2424           2 :   bool res = sch_arm_.call(srv);
+    2425             : 
+    2426           2 :   if (res) {
+    2427             : 
+    2428           2 :     if (!srv.response.success) {
+    2429           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call disarming returned: %s.", srv.response.message.c_str());
+    2430             :     }
+    2431             : 
+    2432             :   } else {
+    2433           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for disarming failed!");
+    2434             :   }
+    2435           2 : }
+    2436             : 
+    2437             : //}
+    2438             : 
+    2439             : /* switchControllerSrv() //{ */
+    2440             : 
+    2441          80 : bool UavManager::switchControllerSrv(const std::string& controller) {
+    2442             : 
+    2443          80 :   ROS_INFO_STREAM("[UavManager]: activating controller '" << controller << "'");
+    2444             : 
+    2445         160 :   mrs_msgs::String srv;
+    2446          80 :   srv.request.value = controller;
+    2447             : 
+    2448          80 :   bool res = sch_switch_controller_.call(srv);
+    2449             : 
+    2450          80 :   if (res) {
+    2451             : 
+    2452          80 :     if (!srv.response.success) {
+    2453           0 :       ROS_WARN("[UavManager]: service call for switching controller returned: '%s'", srv.response.message.c_str());
+    2454             :     }
+    2455             : 
+    2456          80 :     return srv.response.success;
+    2457             : 
+    2458             :   } else {
+    2459             : 
+    2460           0 :     ROS_ERROR("[UavManager]: service call for switching controller failed!");
+    2461             : 
+    2462           0 :     return false;
+    2463             :   }
+    2464             : }
+    2465             : 
+    2466             : //}
+    2467             : 
+    2468             : /* switchTrackerSrv() //{ */
+    2469             : 
+    2470          82 : bool UavManager::switchTrackerSrv(const std::string& tracker) {
+    2471             : 
+    2472          82 :   ROS_INFO_STREAM("[UavManager]: activating tracker '" << tracker << "'");
+    2473             : 
+    2474             : 
+    2475         164 :   mrs_msgs::String srv;
+    2476          82 :   srv.request.value = tracker;
+    2477             : 
+    2478          82 :   bool res = sch_switch_tracker_.call(srv);
+    2479             : 
+    2480          82 :   if (res) {
+    2481             : 
+    2482          82 :     if (!srv.response.success) {
+    2483           0 :       ROS_WARN("[UavManager]: service call for switching tracker returned: '%s'", srv.response.message.c_str());
+    2484             :     }
+    2485             : 
+    2486          82 :     return srv.response.success;
+    2487             : 
+    2488             :   } else {
+    2489             : 
+    2490           0 :     ROS_ERROR("[UavManager]: service call for switching tracker failed!");
+    2491             : 
+    2492           0 :     return false;
+    2493             :   }
+    2494             : }
+    2495             : 
+    2496             : //}
+    2497             : 
+    2498             : /* landSrv() //{ */
+    2499             : 
+    2500           2 : bool UavManager::landSrv(void) {
+    2501             : 
+    2502           2 :   ROS_INFO("[UavManager]: calling for landing");
+    2503             : 
+    2504           4 :   std_srvs::Trigger srv;
+    2505             : 
+    2506           2 :   bool res = sch_land_.call(srv);
+    2507             : 
+    2508           2 :   if (res) {
+    2509             : 
+    2510           2 :     if (!srv.response.success) {
+    2511           0 :       ROS_WARN("[UavManager]: service call for landing returned: '%s'", srv.response.message.c_str());
+    2512             :     }
+    2513             : 
+    2514           2 :     return srv.response.success;
+    2515             : 
+    2516             :   } else {
+    2517             : 
+    2518           0 :     ROS_ERROR("[UavManager]: service call for landing failed!");
+    2519             : 
+    2520           0 :     return false;
+    2521             :   }
+    2522             : }
+    2523             : 
+    2524             : //}
+    2525             : 
+    2526             : /* elandSrv() //{ */
+    2527             : 
+    2528           0 : bool UavManager::elandSrv(void) {
+    2529             : 
+    2530           0 :   ROS_INFO("[UavManager]: calling for eland");
+    2531             : 
+    2532           0 :   std_srvs::Trigger srv;
+    2533             : 
+    2534           0 :   bool res = sch_eland_.call(srv);
+    2535             : 
+    2536           0 :   if (res) {
+    2537             : 
+    2538           0 :     if (!srv.response.success) {
+    2539           0 :       ROS_WARN("[UavManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    2540             :     }
+    2541             : 
+    2542           0 :     return srv.response.success;
+    2543             : 
+    2544             :   } else {
+    2545             : 
+    2546           0 :     ROS_ERROR("[UavManager]: service call for eland failed!");
+    2547             : 
+    2548           0 :     return false;
+    2549             :   }
+    2550             : }
+    2551             : 
+    2552             : //}
+    2553             : 
+    2554             : /* ehoverSrv() //{ */
+    2555             : 
+    2556           0 : bool UavManager::ehoverSrv(void) {
+    2557             : 
+    2558           0 :   ROS_INFO("[UavManager]: calling for ehover");
+    2559             : 
+    2560           0 :   std_srvs::Trigger srv;
+    2561             : 
+    2562           0 :   bool res = sch_ehover_.call(srv);
+    2563             : 
+    2564           0 :   if (res) {
+    2565             : 
+    2566           0 :     if (!srv.response.success) {
+    2567           0 :       ROS_WARN("[UavManager]: service call for ehover returned: '%s'", srv.response.message.c_str());
+    2568             :     }
+    2569             : 
+    2570           0 :     return srv.response.success;
+    2571             : 
+    2572             :   } else {
+    2573             : 
+    2574           0 :     ROS_ERROR("[UavManager]: service call for ehover failed!");
+    2575             : 
+    2576           0 :     return false;
+    2577             :   }
+    2578             : }
+    2579             : 
+    2580             : //}
+    2581             : 
+    2582             : /* takeoffSrv() //{ */
+    2583             : 
+    2584           6 : bool UavManager::takeoffSrv(void) {
+    2585             : 
+    2586           6 :   ROS_INFO("[UavManager]: calling for takeoff to height '%.2f m'", _takeoff_height_);
+    2587             : 
+    2588          12 :   mrs_msgs::Vec1 srv;
+    2589             : 
+    2590           6 :   srv.request.goal = _takeoff_height_;
+    2591             : 
+    2592           6 :   bool res = sch_takeoff_.call(srv);
+    2593             : 
+    2594           6 :   if (res) {
+    2595             : 
+    2596           6 :     if (!srv.response.success) {
+    2597           0 :       ROS_WARN("[UavManager]: service call for takeoff returned: '%s'", srv.response.message.c_str());
+    2598             :     }
+    2599             : 
+    2600           6 :     return srv.response.success;
+    2601             : 
+    2602             :   } else {
+    2603             : 
+    2604           0 :     ROS_ERROR("[UavManager]: service call for takeoff failed!");
+    2605             : 
+    2606           0 :     return false;
+    2607             :   }
+    2608             : }
+    2609             : 
+    2610             : //}
+    2611             : 
+    2612             : /* emergencyReferenceSrv() //{ */
+    2613             : 
+    2614           0 : bool UavManager::emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal) {
+    2615             : 
+    2616           0 :   ROS_INFO_THROTTLE(1.0, "[UavManager]: calling for emergency reference");
+    2617             : 
+    2618           0 :   mrs_msgs::ReferenceStampedSrv srv;
+    2619             : 
+    2620           0 :   srv.request.header    = goal.header;
+    2621           0 :   srv.request.reference = goal.reference;
+    2622             : 
+    2623           0 :   bool res = sch_emergency_reference_.call(srv);
+    2624             : 
+    2625           0 :   if (res) {
+    2626             : 
+    2627           0 :     if (!srv.response.success) {
+    2628           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call for emergency reference returned: '%s'", srv.response.message.c_str());
+    2629             :     }
+    2630             : 
+    2631           0 :     return srv.response.success;
+    2632             : 
+    2633             :   } else {
+    2634             : 
+    2635           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for emergency reference failed!");
+    2636             : 
+    2637           0 :     return false;
+    2638             :   }
+    2639             : }
+    2640             : 
+    2641             : //}
+    2642             : 
+    2643             : }  // namespace uav_manager
+    2644             : 
+    2645             : }  // namespace mrs_uav_managers
+    2646             : 
+    2647             : #include <pluginlib/class_list_macros.h>
+    2648          42 : 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..c4d7df73e5 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html @@ -0,0 +1,682 @@ + + + + + + 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 + + +
+ 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..dc325ad1e03d15a2f41010360a75634271d0f6e2 GIT binary patch literal 7136 zcmV<68z1C}P)4V{WxQN2)8BtX zul9$}AidO3MnD0uvg=oyx&!v;THG+k9(WUsLO?M_dDrqW^1zLN;<+w>QZqxsi>Hvm znA1c9N-Hv{U}BkX#k;$#@Nq02BTq(=-WV?hptB} zk!A$FFdBSyKtr?uV}Gg#vHVE{0s?Lynr|XmR5inIk9Hh8;dDkI*xn9fbfzM3VAD|L zWAbK6;Q8QRn7A&ym~A(;5up&xC2P|*tE=gn_$Jtd1$O0Qya;z|9me?L$JkN7lGSaT zqWoH_^%(gk+AdOKbSA*q=*gfN?GkXA;1cA1I)U#4lI*`8OqJEbU4zs zhgv=9jb;kSb)E2<=~>4$NLKHi;veC zA0Mi94V7Uat3?RFNmN!+93#vQjzyuUMb|07f^rSXUV<@F0HgmFUCDyyt4lT>u&21V z=bBI=2ElP%XVJ>i%pky$A-xDt5^BwQacKr&W3#-L0D1~){pK!d2X{}FwIGBdi~;?2 zF(?j`BpJguOZkQBo6VdNZX6LoA^~aX5jlno;3lfGtQ2id%H%8njGe81D$Xf|;PN9@zjK4oG zy!iZKqz0L@bGQoCP%hzcjHN-cW6T$%J;%8GeMv8^r7`A!JB*QSN!Kjgk^l3`F;ERVVRL_)5)U~hxD8g7;uT@l!E^E3TT1*uqPVr zGSvuJI7SsQ9cE_NG42D{i)t@ub-?*6$0z`tdlD0UF1cR+->$(rSHrkOz>ghc0Pbl> zy*zfI8sJuKXJY7cyoAk72!pEq?=sa37$e@1GAu%Tgu6EiP|inU!6&wc!~OmQZGHW} zJ|BPo*>?900N#sjxIfAO0Z`u139-?i*xNpZw5ztDDJwBQ7S!@0l<|IO$LA; znrj+gAu-M1PY)wyd6{cs>T|i#q=3|Bc%d;8{xt(gy3I@$;sf~TI;aV5FpB;3dXWNp zV4*dW+sA$wuVSioM(U65zI=SOI^a^r*e1CCz*(kS_#ZB7{DN_0w{o{Gu zJVxxJY~rKA&rbDn(*stt70RO+@K#)y6kX>H^njhS-Wa8GeMPP;-S@oY7@0vXDs>G4 zR*@by(RJ(LhO`l<1aXjOMIVJEy{!hOeTTT~I_*iqQ#1xYCWC z>lbuf%<&+aiMdjPrvODZKrXnpXVDl1P|B{6YLvPbk5OhJce+xNO8e(jr^aEie-F@R!|B5u2wl(FN+ zY#`J%*HksbVZlZL&?LkJxvoK$4*LUqtEE6kAP8LcC%Q7S`hIH|5^tEKd4Xe$vx3sD z6fqPaDvDLG2XH*=u?Zu}8OLg&g!qA#h5u1-`Z9=oXauYZVyda%$FpJqHMA5TWo-}pxS)U=)EhKX-56IpC$AY; z*Js+o7;AvG$%!%Sv1lN+4eUV}3ygmOJ|qJ4>e`@tJf3umfHHwJW8{$i{b0?y_CE=_ zgXY09TqI}mF>3Uhkb_4Yq8qK5LINt|tY)t7fEx%)1}Mk4+96c|#Te;1bmq?U>IEp8 z8qLi7VP085v$CdVcxbCf;)pOsQ|8}Rj1eaVD8o2NV;^_WcP7DL0III-?%t=ZC(7BM zhFld^LTc~1^t^`kAcmL&{^7EK#;n!=DPsNyU}{od@~Bs?O!*-~)07wm7C?tFwR_Jv zHKFBz-hMt%F`|MPqYKp}m!_dHKKKR40j_o#zzfz2O%lnT$HPrQicz#)tBA=u&Kn`P zLM?p#EW_vlyBm8#$cR36*a7f6QoME6WGZZdtFxr|%W${Ocp0wbjy-RUX_rDON1b-L zxG_(c@T5kvPQ9QZ7mLI|sy)};*CxTbsHl!(OwPJxbA3(IQt2?QQ>jZ0r6F3fNLz=o zRG-cRh8j!)qa@%f*O{beG>41*V%CM4hTg_L=gxAYBCmsxJ4*H>Vg|J`d{--Gd7}Xv zF_KJZR)x|TaScW)A(y1l+c4Gvchwn=l{1+@LX0CS9zh^9)+ z_&I#ouOA=e_&mKvFbvgFbzF>O4 zM=T|a6htPVN&;QH2T{zI0?Sh3a=~RS#RC{$mrW7L*$yT}-2B88tDmnQG}kv%jA}61 zzhZG}E5=M!ny4-uIcY2K%P{hQa*T;)mRO_z=DOx$>1FJ?kv#!*4)|88&3Q&d zFinhAj^r#pMT~TeA;dNMYaMCzPV2+@<--l1yK*uG%w{>i)=^3fSEHHspgSTO?4Eo= zi*v25(ew;rccju9P1Yl|pVw!sL6Z72JCt7f4J7C@et(EcfVEAl2V`x0y4#al z3SlAOeaHh>ekxHR;Qjs8vvcG*&qpbH*m5&pGyGV!^=632T)9~Wn3jgHfJ|wKx`Y)s z7q0IP7^!;bk|b2k4n_6`j69$Mqt2h_z58rvw$qTscf{5JkhOM7`F;>f!TubC60xXLFs~Yy3NV%_P}nlCtwCu zDcg>*zS|LJ{qWNfhwXI4BgWJ5!K>J@9UtB$@9OwGbtN&Uh)W?c1O;_+utLo0iR%QOmbLB*L5g#g$p)O@v2`HidP`q^W~#*pi|Se zH=%aZ8VmB^ZrM2KmS}Q?$a_EfixmPX+t%P!HiJma(43{SC?`g88po=_&Uc4( z=VO7UMkf~rQy(Cv>MkSl4O_Lu#;Iqf%J%Q|h7Z4!`buxObml_|DDRpW`h0bpG`mro zb;MlZPRL`=D>2jiA3aYgKtVJH|)YM#f^M+T4Q}sX3r||KAKjT5W=6o$LmK+7nTv1X zMLOzJL)D69biL71xbGL=gH!>OzAEvyvpYz_3ru>ugcr+0Z0jRtP}p`BGh(e=XuzCi z1b6^3qje-3^&2r#sMHw6 zYIw!=k+o{gM{T>DsbFqme_;1sch83;?9zg<9R5&a6oen-2aNj%AX>zXf2X2Z%=%QY zonlr7e3zKz9{AR|eq){sT@jVerM-<@J%wq!Gs>-(+{XrteRAv)u~lR2T0MJ^@NBVh zue)WZf_Hu04Zn%*uO##cH$D#9rACX|H4~>*_SiLl*_94?3+p@tJeqU1f9}IG>$O8Z zoO6~m-1RZ$8a_AdUN4?{8L8dsNC^>A-6y4zo>C3)Ch76F7SGA0&UTDq>GAv?JMmk- za|PkvUQd|gKn3qM-`y(}agRscc}W%7gvti|SVgQ>d=xi(31CGiYBDAFIyw)mKwHoi*a|tLyav(g5;{?_I%R5M>C6ZY}baiS7>_ z7jC?dw8|bCe|*q|85$twop-v(8o*e1l+*zdkCKv4-$;}-0@iFj2CNfPxAprRVgUku zb5|Wvb1fEJ$%Ab=jDxQ~4uNJ)C6w{UC5k>>QxWRyBQ-3blo)gDd~}4EXT~_1Z1J`E zaX6mtA3{Zi*da@3PqY`PmQ)sL7j}UfhAmfEH1R9t=Ci+#RHTO5os?{ECR46_jPww+ z7`yKFc36f-q=bA2OpLa~IKeWH(Pvbn*|+kDM83;w)owkBYfDh`oS4zwnKHS>hF)&`=;H+|9+o{o zROchGJSkH^)7YMv%jNIg*P4{cF>eZ)@zH$=yN3RCk150q8vTBNB3$Y#0Bf5X+SW&j zlAWRZN%BfJ0SK*r5#vsP(VGK+(sLhTdBi;z`6%l8DonE5Ooj7YYsq+vP6JpoQVFZW z%%IW+J=NKh?&YqffCaQ#b&W<$C=`93(<5D(EzipAc_D~!gJgC>u~3)UDtz_^X$b2_ zkc;p&GUlP{8pbzYj#W`WD?P_3b=&;`Q)gU~Ps%pN(OQkrdgxQ0p4L+A%rOl zg+RTNO=A0a!~zz&4uIhJ3aF1%?~|XYa{Pb=EJX2&Bj;BWVseJJ6tDz&MMYPEm>;wh z!nbM}J`x=GGCm$CL2GiVg%>)sSt*OF6QHK+FWe0G+74$|%q1G(KPe)^@mFhaOFuhr zhLb$^3X6)`Furj!tjMuT*#oO~UVDQ=kxeko=HZhEsMAUTzsEF78?Jt0YM<^2A5W*E zT^H{6{I*|;?**qKKNj3|dn!@=%uH=F!@Fk0pYK~VQ|9FDo-3Eo zQoMeaVf29AEBC>Xs^YVsO>wj0v!6PX;&>xFr4f!*@#Ue(6b2UHAe(xO=6zi$U=dWg z#D&7hLLm^Oy7aW92Q(iffQhVZAtqa0nGs}D2n4uG*E~&)82ik1_a}t3%C)*^Dl5M^ zAL-SjO}PX|phU{GhanX#M}HWFJuAB2`GV{&2?I7igA_Pp;qvsmfOE0h7-_;}_OQou7Fe)xEJ78$-ja z40E+oea6ko?D>eeS;4VC<7Uq@ZdUD@r|FEFt);m188<6qJY3wYOKo%EVL9}pBOdZ| z2mEf?74+vJKl;p0oEdm`ARjr?`U?Hsl(%?sjknKRK2jT?laEEI%=zf<8e*wF`6#$k z+Wq72CPLULoc1iaBlcU z(<6fJ0>)lr3^vmlQP`j{IIpv=TxMs;p1^&(?rO8i-IwXEMIHS0q1@%7km|3x&RS-1 zJ;shTaeh?*17ylW-?%D#@#WL+(wQ%~&RQekzo;`3jcL0yd8Y38Q%et&$AUOE-i2ID z4qwxIU6}vl`^ zjL>m_GK?Q{I#m#-I2U%Z9Uzo4K>xf?;y#!Wq*+ke7Og7ycNoBvNF*oF;?ZR@?A1D1 zVK!W^?pCv${AR*Nv<(yUa6hc`iRk9BDdk{W`Ep&sd+>PFLT z0hJp~ADW4=3i>5a|6?Ea4UfD)IGo2RdBf5tdV}j~K5{{B7v=`iQv#HC{h|38k7C`; zCH?rw@N|hn>vIT1YlsXdzEA61_m2$cbG>!Zytm2Y!!Oy7bOb*wkC`DQ`mu3g{2KBU z<=7|4upuo*EE?5vkfIQk(i`zT9SJfG8Tnq9j6SI3yW51rpK0G82f*9_D zQKR(X_72kHx)wbB>hnQUcrulFk6FJU0oB?>3{bApdbCNoieh#!Equ~P(0*47!~TB+ zD_AbLNC}h2aht>}J~kR&qV_K+$A1X}P)u~ox)vU^Ge;ZU3$!a?UR%yRiZj-8_|^k)ffvE zBx;N^;izv(HW^k)0L2(_fd-2^SruRaJshGlf9+7>FAjX-h0C7YvC+z~;+enXXYIu% zMa=MhBXNB~akK$g+=22Is!0RjzL*W>#VFoS7=TR}KhOZgGjgA603P3O0Q%k2kXiFB;r~yQr1w-!P30hNF^p^ z^XY=jtZyA-eHpepHH9h&+YXlpcC?Rv^Kk_G_>DE}{Z5Hzwa3f_)F0)2Fo+N6eXN01 zpz7A2CH-LU!+!HCy^oH;f^=bLZ(tFbvg<*8g&r6`#8*hw^%?&9E_FRnZ0|n=3&JK^ zeV!{#OULx^E4$}D&AN+pqmLj1ELCW0iTR0`J&3+O_Rwo|fVPW$-1DMw#+JBB71ajm zhl2(Ew(NwFTB31@-fssZwfM>w>yN3fOOUlzYgPbMP@P*dVh~?p&HNXqHKVLqNVJ{S ztb%du>>#DqjF)h+^rWHBg{h4Dq|j%Nz1b()9&dJBWx;8CvNTHb>qS3mdwO1vnS?%z zFUAUICR2p$m{cpq{er0-Fn&ld71|zTdiJnAxNx7Jop)w@SliQHTJ*!V=U8QA++748 zWqZD>j0}~1``YT$_9V6^IplQOo(jgXbH+D|AaNBFTYWAQH?pPiu0uV>N?fQl&sA_2 zwnUJ&kMZ>4PA@J6{6cHiF>99xpVqdW6iWDl^oKH9hzG~+r&=(E_ou5}hf5Qg z1IF^M(`p>D{{wE~{%bYP;Wlx6Wx?Y#d*wE{n_TTyt~*BRFuuCM!`4Ntw@UK`$NvCY W*`Uc(Jqw`#0000 + + + + + + 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:2023-12-06 07:59:45Functions: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()35
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()35
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().235
+
+
+ + + +
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..0bec9b24fb --- /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:2023-12-06 07:59:45Functions: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()35
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()35
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().235
+
+
+ + + +
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..fa58811947 --- /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:2023-12-06 07:59:45Functions: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          35 :   GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) {
+      57          35 :   }
+      58             : 
+      59          70 :   ~GarminAgl(void) {
+      60          70 :   }
+      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:2023-12-06 07:59:45Functions: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..2c44eeee37 --- /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:2023-12-06 07:59:45Functions: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..1329dac2d6 --- /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:2023-12-06 07:59:45Functions: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..aac2fab2f1 --- /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:2023-12-06 07:59:45Functions: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..c2ea9a7009 --- /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:2023-12-06 07:59:45Functions: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..d77bca1163 --- /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:2023-12-06 07:59:45Functions: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..8ee6f3f71a --- /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:2023-12-06 07:59:45Functions: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)74
mrs_uav_state_estimators::AltGeneric::~AltGeneric()74
mrs_uav_state_estimators::AltGeneric::~AltGeneric().274
+
+
+ + + +
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..651559aaaa --- /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:2023-12-06 07:59:45Functions: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)74
mrs_uav_state_estimators::AltGeneric::~AltGeneric()74
mrs_uav_state_estimators::AltGeneric::~AltGeneric().274
+
+
+ + + +
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..a9c8f947c1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html @@ -0,0 +1,242 @@ + + + + + + + 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:2023-12-06 07:59:45Functions: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 A_t        = lkf_t::A_t;
+      46             :   using B_t        = lkf_t::B_t;
+      47             :   using H_t        = lkf_t::H_t;
+      48             :   using Q_t        = lkf_t::Q_t;
+      49             :   using x_t        = lkf_t::x_t;
+      50             :   using P_t        = lkf_t::P_t;
+      51             :   using u_t        = lkf_t::u_t;
+      52             :   using z_t        = lkf_t::z_t;
+      53             :   using R_t        = lkf_t::R_t;
+      54             :   using statecov_t = lkf_t::statecov_t;
+      55             : 
+      56             :   typedef mrs_lib::Repredictor<lkf_t> rep_lkf_t;
+      57             : 
+      58             :   using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;
+      59             : 
+      60             : private:
+      61             :   std::string parent_state_est_name_;
+      62             : 
+      63             :   double                              dt_;
+      64             :   double                              input_coeff_, 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<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(AltitudeEstimatorConfig& config, [[maybe_unused]] uint32_t level);
+      78             : 
+      79             :   z_t                innovation_;
+      80             :   mutable std::mutex mtx_innovation_;
+      81             : 
+      82             :   bool is_error_state_first_time_ = true;
+      83             :   ros::Duration error_state_duration_;
+      84             :   ros::Time prev_time_in_error_state_;
+      85             : 
+      86             :   bool is_repredictor_enabled_;
+      87             :   int  rep_buffer_size_ = 200;
+      88             : 
+      89             :   const bool is_core_plugin_;
+      90             : 
+      91             :   std::vector<std::string>                                              correction_names_;
+      92             :   std::vector<std::shared_ptr<Correction<alt_generic::n_measurements>>> corrections_;
+      93             : 
+      94             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput> sh_control_input_;
+      95             :   void                                                timeoutCallback(const std::string &topic, const ros::Time &last_msg);
+      96             :   std::atomic<bool>                                   is_input_ready_ = false;
+      97             : 
+      98             :   ros::Timer timer_update_;
+      99             :   void       timerUpdate(const ros::TimerEvent &event);
+     100             : 
+     101             :   ros::Timer timer_check_health_;
+     102             :   void       timerCheckHealth(const ros::TimerEvent &event);
+     103             : 
+     104             :   void doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id);
+     105             :   void doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp);
+     106             : 
+     107             :   bool isConverged();
+     108             : 
+     109             :   Q_t                getQ();
+     110             :   mutable std::mutex mtx_Q_;
+     111             : 
+     112             : public:
+     113          74 :   AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     114          74 :       : AltitudeEstimator<alt_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     115          74 :   }
+     116             : 
+     117         148 :   ~AltGeneric(void) {
+     118         148 :   }
+     119             : 
+     120             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     121             :   bool start(void) override;
+     122             :   bool pause(void) override;
+     123             :   bool reset(void) override;
+     124             : 
+     125             :   double getState(const int &state_idx_in) const override;
+     126             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     127             : 
+     128             :   void setState(const double &state_in, const int &state_idx_in) override;
+     129             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     130             : 
+     131             :   states_t getStates(void) const override;
+     132             :   void     setStates(const states_t &states_in) override;
+     133             : 
+     134             :   double getCovariance(const int &state_idx_in) const override;
+     135             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     136             : 
+     137             :   covariance_t getCovarianceMatrix(void) const override;
+     138             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     139             : 
+     140             :   double getInnovation(const int &state_idx) const override;
+     141             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     142             : 
+     143             :   void setDt(const double &dt);
+     144             :   void setInputCoeff(const double &input_coeff);
+     145             : 
+     146             :   void generateA();
+     147             :   void generateB();
+     148             : 
+     149             :   void timeoutOdom(const std::string &topic, const ros::Time &last_msg, const int n_pubs);
+     150             :   void timeoutRange(const std::string &topic, const ros::Time &last_msg, const int n_pubs);
+     151             : 
+     152             :   std::string getNamespacedName() const;
+     153             : 
+     154             :   std::string getPrintName() const;
+     155             : };
+     156             : }  // namespace mrs_uav_state_estimators
+     157             : 
+     158             : #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..872f0abb3c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_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/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 + + +
+ 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..fdd77c42987a7bc3630f5b90c60f6d87d72bbc76 GIT binary patch literal 682 zcmV;b0#*HqP)(mad{$f%qh)u%DK)n18Omi@VH(-O z)_}`B6W2G~-*Anm>(3a#TneMWW6mw0+Yr#5A)jS30HK+ue@s(ebjWF^N?g&FDXkhc zG~zLTeZWj>n!`|lHse2V4KNqve8q6arh4#3f`bu-$|g1;(RmLEU`=*H?^@9!0UI@^ zm|l~j@|VDFvh+i4168u#6@k{4`U+GgvM(ilW+X-t>oXb$SXA0+25Diyh?J6uyE9+$ zkQFRg3M?2zjk7C!Ex;D`{4v0J3_tQ%@`7ugeo`pQl6^9|GAJxAvEYWv%p@X;bXImx zy;K(RwOpYq0C zFMGUT-OP4Py4e!X-YB;BWv~3Xr@wdpyspExZTnlc7ySy`zPT*;Ulg;WflYMLA$uz%m|-ns!A!oyX6teHI$8v@6Ddi^C>a(n?R5I16zQm+97J9 QqyPW_07*qoM6N<$f@O^^S^xk5 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..e76451f16c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.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/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:2023-12-06 07:59:45Functions: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&)74
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().274
+
+
+ + + +
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..0dfdb5abfa --- /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:2023-12-06 07:59:45Functions: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&)74
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().274
+
+
+ + + +
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..3bd4359cba --- /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:2023-12-06 07:59:45Functions: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          74 :   AltitudeEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(altitude::type, name, frame_id) {
+      31          74 :   }
+      32             : 
+      33          74 :   virtual ~AltitudeEstimator(void) {
+      34          74 :   }
+      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..2907d6ff4b --- /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:2023-12-06 07:59:45Functions: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..7fe07fcfaf --- /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:2023-12-06 07:59:45Functions: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..36f757b4c1 --- /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:2023-12-06 07:59:45Functions: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..ce607805c4 --- /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:2023-12-06 07:59:45Functions: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..439f781c37 --- /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:2023-12-06 07:59:45Functions: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..fcc18d6966 --- /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:2023-12-06 07:59:45Functions: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..cd8bc09fef --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html @@ -0,0 +1,348 @@ + + + + + + + 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:37065556.5 %
Date:2023-12-06 07:59:45Functions:496773.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::getVelInFrame(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>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<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>::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>::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<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)32
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)39
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)>)41
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const82
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const83
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)>)113
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const165
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const226
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const259
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const347
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)994
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)997
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1031
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1031
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1043
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1043
mrs_uav_state_estimators::Correction<2>::getRawCorrection()1219
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()1219
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()2740
mrs_uav_state_estimators::Correction<1>::getRawCorrection()2741
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4466
mrs_uav_state_estimators::Correction<2>::getVelInFrame(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> >)4470
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4470
mrs_uav_state_estimators::Correction<2>::isMsgComing()50591
mrs_uav_state_estimators::Correction<2>::isHealthy()50591
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)83340
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)83437
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)97235
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)97272
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)99651
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)99665
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)99690
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)99690
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)101011
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)102692
mrs_uav_state_estimators::Correction<2>::setR(double)102776
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)102777
mrs_uav_state_estimators::Correction<2>::getStateId() const102861
mrs_uav_state_estimators::Correction<1>::isHealthy()137585
mrs_uav_state_estimators::Correction<1>::isMsgComing()137639
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)205553
mrs_uav_state_estimators::Correction<1>::getStateId() const282619
mrs_uav_state_estimators::Correction<1>::setR(double)282621
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)282920
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)283556
mrs_uav_state_estimators::Correction<2>::getR()513797
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)566074
mrs_uav_state_estimators::Correction<1>::getR()848512
+
+
+ + + +
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..43b226e53f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html @@ -0,0 +1,348 @@ + + + + + + + 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:37065556.5 %
Date:2023-12-06 07:59:45Functions:496773.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)994
mrs_uav_state_estimators::Correction<1>::isMsgComing()137639
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)83340
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)99665
mrs_uav_state_estimators::Correction<1>::getVelInFrame(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>)99651
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&)282920
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)99690
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()2741
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)566074
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>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1031
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>)83437
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)101011
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()2740
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)32
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)99690
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()848512
mrs_uav_state_estimators::Correction<1>::setR(double)282621
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)283556
mrs_uav_state_estimators::Correction<1>::isHealthy()137585
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)>)113
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)997
mrs_uav_state_estimators::Correction<2>::isMsgComing()50591
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)97235
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getVelInFrame(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> >)4470
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4466
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&)102692
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()1219
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)205553
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>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1043
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>)97272
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()1219
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)39
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)4470
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()513797
mrs_uav_state_estimators::Correction<2>::setR(double)102776
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)102777
mrs_uav_state_estimators::Correction<2>::isHealthy()50591
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)>)41
mrs_uav_state_estimators::Correction<1>::getStateId() const282619
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const259
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const347
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1031
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const226
mrs_uav_state_estimators::Correction<2>::getStateId() const102861
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const83
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const165
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1043
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const82
+
+
+ + + +
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..784e26e357 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html @@ -0,0 +1,1795 @@ + + + + + + + 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:37065556.5 %
Date:2023-12-06 07:59:45Functions:496773.1 %
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 <nav_msgs/Odometry.h>
+      22             : 
+      23             : #include <std_srvs/SetBool.h>
+      24             : 
+      25             : #include <functional>
+      26             : 
+      27             : #include <mrs_uav_managers/estimation_manager/types.h>
+      28             : #include <mrs_uav_managers/estimation_manager/support.h>
+      29             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      30             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      31             : 
+      32             : #include <mrs_uav_state_estimators/processors/processor.h>
+      33             : #include <mrs_uav_state_estimators/processors/proc_median_filter.h>
+      34             : #include <mrs_uav_state_estimators/processors/proc_saturate.h>
+      35             : #include <mrs_uav_state_estimators/processors/proc_excessive_tilt.h>
+      36             : #include <mrs_uav_state_estimators/processors/proc_tf_to_world.h>
+      37             : 
+      38             : #include <mrs_uav_state_estimators/CorrectionConfig.h>
+      39             : 
+      40             : 
+      41             : namespace mrs_uav_state_estimators
+      42             : {
+      43             : 
+      44             : typedef enum
+      45             : {
+      46             :   LATERAL,
+      47             :   ALTITUDE,
+      48             :   HEADING
+      49             : } EstimatorType_t;
+      50             : const int n_EstimatorType_t = 3;
+      51             : 
+      52             : typedef enum
+      53             : {
+      54             :   UNKNOWN,
+      55             :   ODOMETRY,
+      56             :   POSE,
+      57             :   POSECOV,
+      58             :   RANGE,
+      59             :   RTK_GPS,
+      60             :   POINT,
+      61             :   VECTOR,
+      62             :   QUAT,
+      63             : } MessageType_t;
+      64             : const int n_MessageType_t = 9;
+      65             : 
+      66             : const std::map<std::string, MessageType_t> map_msg_type{{"nav_msgs/Odometry", MessageType_t::ODOMETRY},
+      67             :                                                         {"geometry_msgs/PoseStamped", MessageType_t::POSE},
+      68             :                                                         {"geometry_msgs/PoseWithCovarianceStamped", MessageType_t::POSECOV},
+      69             :                                                         {"sensor_msgs/Range", MessageType_t::RANGE},
+      70             :                                                         {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS},
+      71             :                                                         {"geometry_msgs/PointStamped", MessageType_t::POINT},
+      72             :                                                         {"geometry_msgs/Vector3Stamped", MessageType_t::VECTOR},
+      73             :                                                         {"geometry_msgs/QuaternionStamped", MessageType_t::QUAT}};
+      74             : 
+      75             : template <int n_measurements>
+      76             : class Correction {
+      77             : 
+      78             :   using CommonHandlers_t  = mrs_uav_managers::estimation_manager::CommonHandlers_t;
+      79             :   using PrivateHandlers_t = mrs_uav_managers::estimation_manager::PrivateHandlers_t;
+      80             :   using StateId_t         = mrs_uav_managers::estimation_manager::StateId_t;
+      81             : 
+      82             : public:
+      83             :   typedef Eigen::Matrix<double, n_measurements, 1>         measurement_t;
+      84             :   typedef mrs_lib::DynamicReconfigureMgr<CorrectionConfig> drmgr_t;
+      85             : 
+      86             :   struct MeasurementStamped
+      87             :   {
+      88             :     ros::Time     stamp;
+      89             :     measurement_t value;
+      90             :   };
+      91             : 
+      92             : public:
+      93             :   Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& frame_id, const EstimatorType_t& est_type,
+      94             :              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+      95             :              std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction);
+      96             : 
+      97             :   std::string getName() const;
+      98             :   std::string getNamespacedName() const;
+      99             :   std::string getPrintName() const;
+     100             : 
+     101             :   double    getR();
+     102             :   void      setR(const double R);
+     103             :   StateId_t getStateId() const;
+     104             : 
+     105             :   bool             isHealthy();
+     106             :   ros::Time        healthy_time_;
+     107             :   std::atomic_bool is_healthy_    = true;
+     108             :   std::atomic_bool is_delay_ok_   = true;
+     109             :   std::atomic_bool is_dt_ok_      = true;
+     110             :   std::atomic_bool is_nan_free_   = true;
+     111             :   std::atomic_bool got_first_msg_ = false;
+     112             : 
+     113             :   int counter_nan_ = 0;
+     114             : 
+     115             :   std::optional<MeasurementStamped> getRawCorrection();
+     116             :   std::optional<MeasurementStamped> getProcessedCorrection();
+     117             : 
+     118             :   void resetProcessors();
+     119             : 
+     120             : private:
+     121             :   std::atomic_bool is_initialized_ = false;
+     122             : 
+     123             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odom_;
+     124             :   void                                          callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     125             :   std::optional<measurement_t>                  getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg);
+     126             : 
+     127             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped> sh_pose_s_;
+     128             :   void                                                  callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg);
+     129             :   std::optional<measurement_t>                          getCorrectionFromPoseStamped(const geometry_msgs::PoseStampedConstPtr msg);
+     130             : 
+     131             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped> sh_pose_wcs_;
+     132             : 
+     133             :   mrs_lib::SubscribeHandler<mrs_msgs::RtkGps> sh_rtk_;
+     134             :   void                                        callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg);
+     135             :   std::optional<measurement_t>                getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg);
+     136             :   void                                        getAvgRtkInitZ(const double rtk_z);
+     137             :   bool                                        got_avg_init_rtk_z_ = false;
+     138             :   double                                      rtk_init_z_avg_     = 0.0;
+     139             :   int                                         got_rtk_counter_    = 0;
+     140             : 
+     141             :   mrs_lib::SubscribeHandler<geometry_msgs::PointStamped> sh_point_;
+     142             :   void                                                   callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg);
+     143             :   std::optional<measurement_t>                           getCorrectionFromPoint(const geometry_msgs::PointStampedConstPtr msg);
+     144             : 
+     145             :   mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped> sh_vector_;
+     146             :   std::optional<measurement_t>                             getCorrectionFromVector(const geometry_msgs::Vector3StampedConstPtr msg);
+     147             :   void                                                     callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg);
+     148             : 
+     149             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_; // for obtaining heading rate
+     150             :   std::string orientation_topic_;
+     151             : 
+     152             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_quat_;
+     153             :   measurement_t                                               prev_hdg_measurement_;
+     154             :   bool                                                        got_first_hdg_measurement_ = false;
+     155             : 
+     156             :   mrs_lib::SubscribeHandler<sensor_msgs::Range> sh_range_;
+     157             :   std::optional<measurement_t>                  getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg);
+     158             :   void                                          callbackRange(const sensor_msgs::Range::ConstPtr msg);
+     159             :   ros::ServiceServer                            ser_toggle_range_;
+     160             :   bool                                          callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     161             :   bool                                          range_enabled_ = true;
+     162             : 
+     163             :   void applyCorrection(const measurement_t& meas, const ros::Time& stamp);
+     164             : 
+     165             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_raw_;
+     166             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_proc_;
+     167             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>      ph_delay_;
+     168             : 
+     169             :   const std::string                  est_name_;
+     170             :   const std::string                  name_;
+     171             :   const std::string                  ns_frame_id_;
+     172             :   const EstimatorType_t              est_type_;
+     173             :   std::shared_ptr<CommonHandlers_t>  ch_;
+     174             :   std::shared_ptr<PrivateHandlers_t> ph_;
+     175             : 
+     176             :   MessageType_t msg_type_;
+     177             :   std::string   msg_topic_;
+     178             :   double        msg_timeout_;
+     179             : 
+     180             :   double     R_;
+     181             :   double     default_R_;
+     182             :   double     R_coeff_;
+     183             :   std::mutex mtx_R_;
+     184             :   StateId_t  state_id_;
+     185             :   bool       is_in_body_frame_ = true;
+     186             : 
+     187             :   std::unique_ptr<drmgr_t> drmgr_;
+     188             : 
+     189             :   std::optional<measurement_t> getCorrectionFromQuat(const geometry_msgs::QuaternionStampedConstPtr msg);
+     190             :   std::optional<measurement_t> getZVelUntilted(const geometry_msgs::Vector3& msg, const std_msgs::Header& header);
+     191             :   std::optional<measurement_t> getVelInFrame(const geometry_msgs::Vector3& vel_in, const std_msgs::Header& source_header, const std::string target_frame);
+     192             : 
+     193             :   std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;
+     194             : 
+     195             :   void   checkMsgDelay(const ros::Time& msg_time);
+     196             :   double msg_delay_limit_;
+     197             :   double msg_delay_warn_limit_;
+     198             : 
+     199             :   double time_since_last_msg_limit_;
+     200             : 
+     201             :   std::shared_ptr<Processor<n_measurements>> createProcessorFromName(const std::string& name, ros::NodeHandle& nh);
+     202             :   bool                                       process(measurement_t& measurement);
+     203             : 
+     204             :   bool             isTimestampOk();
+     205             :   bool             isMsgComing();
+     206             :   std::atomic_bool first_timestamp_ = true;
+     207             :   ros::Time        msg_time_;
+     208             :   ros::Time        prev_msg_time_;
+     209             :   std::mutex       mtx_msg_time_;
+     210             : 
+     211             :   std::vector<std::string>                                                    processor_names_;
+     212             :   std::unordered_map<std::string, std::shared_ptr<Processor<n_measurements>>> processors_;
+     213             : 
+     214             :   std::function<double(int, int)>                            fun_get_state_;
+     215             :   std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction_;
+     216             : 
+     217             :   void publishCorrection(const MeasurementStamped& measurement_stamped, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr);
+     218             :   void publishDelay(const double delay);
+     219             : };
+     220             : 
+     221             : /*//{ constructor */
+     222             : template <int n_measurements>
+     223         154 : Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& ns_frame_id,
+     224             :                                        const EstimatorType_t& est_type, const std::shared_ptr<CommonHandlers_t>& ch,
+     225             :                                        const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+     226             :                                        std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction)
+     227             :     : est_name_(est_name),
+     228             :       name_(name),
+     229             :       ns_frame_id_(ns_frame_id),
+     230             :       est_type_(est_type),
+     231             :       ch_(ch),
+     232             :       ph_(ph),
+     233             :       fun_get_state_(fun_get_state),
+     234         154 :       fun_apply_correction_(fun_apply_correction) {
+     235             : 
+     236             :   // | --------------------- load parameters -------------------- |
+     237             : 
+     238         308 :   std::string msg_type_string;
+     239             : 
+     240         154 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + getNamespacedName() + "/");
+     241             : 
+     242         154 :   ph->param_loader->loadParam("message/type", msg_type_string);
+     243         154 :   if (map_msg_type.find(msg_type_string) == map_msg_type.end()) {
+     244           0 :     ROS_ERROR("[%s]: wrong message type: %s of correction %s", getPrintName().c_str(), msg_type_string.c_str(), getName().c_str());
+     245           0 :     ros::shutdown();
+     246             :   }
+     247         154 :   msg_type_ = map_msg_type.at(msg_type_string);
+     248             : 
+     249         154 :   ph->param_loader->loadParam("message/topic", msg_topic_);
+     250         154 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+     251         154 :   ph->param_loader->loadParam("message/limit/delay", msg_delay_limit_);
+     252         154 :   msg_delay_warn_limit_ = msg_delay_limit_ / 2;  // maybe specify this as a param?
+     253         154 :   ph->param_loader->loadParam("message/limit/time_since_last", time_since_last_msg_limit_);
+     254             : 
+     255             :   int state_id_tmp;
+     256         154 :   ph->param_loader->loadParam("state_id", state_id_tmp);
+     257         154 :   if (state_id_tmp < n_StateId_t) {
+     258         154 :     state_id_ = static_cast<StateId_t>(state_id_tmp);
+     259             :   } else {
+     260           0 :     ROS_ERROR("[%s]: wrong state id: %d of correction %s", getPrintName().c_str(), state_id_tmp, getName().c_str());
+     261           0 :     ros::shutdown();
+     262             :   }
+     263             : 
+     264         154 :   if (state_id_ == StateId_t::VELOCITY) {
+     265          41 :     ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
+     266             :   }
+     267             : 
+     268         154 :   ph->param_loader->loadParam("noise", R_);
+     269         154 :   ph->param_loader->loadParam("noise_unhealthy_coeff", R_coeff_);
+     270         154 :   default_R_ = R_;
+     271             : 
+     272             :   // | --------------- processors initialization --------------- |
+     273         154 :   ph->param_loader->loadParam("processors", processor_names_);
+     274             : 
+     275         225 :   for (auto proc_name : processor_names_) {
+     276          71 :     processors_[proc_name] = createProcessorFromName(proc_name, nh);
+     277             :   }
+     278             : 
+     279             :   // | ------------- initialize dynamic reconfigure ------------- |
+     280         154 :   drmgr_               = std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), getPrintName());
+     281         154 :   drmgr_->config.noise = R_;
+     282         154 :   drmgr_->update_config(drmgr_->config);
+     283             : 
+     284             :   // | -------------- initialize subscribe handlers ------------- |
+     285         308 :   mrs_lib::SubscribeHandlerOptions shopts;
+     286         154 :   shopts.nh                 = nh;
+     287         154 :   shopts.node_name          = getPrintName();
+     288         154 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     289         154 :   shopts.threadsafe         = true;
+     290         154 :   shopts.autostart          = true;
+     291         154 :   shopts.queue_size         = 10;
+     292         154 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     293             : 
+     294         154 :   switch (msg_type_) {
+     295           0 :     case MessageType_t::ODOMETRY: {
+     296           0 :       sh_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_, &Correction::callbackOdometry, this);
+     297           0 :       break;
+     298             :     }
+     299           0 :     case MessageType_t::POSE: {
+     300           0 :       sh_pose_s_ = mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped>(shopts, msg_topic_, &Correction::callbackPoseStamped, this);
+     301           0 :       break;
+     302             :     }
+     303           0 :     case MessageType_t::POSECOV: {
+     304             :       // TODO implement
+     305             :       /* sh_pose_wcs_ = mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped>(shopts, msg_topic_); */
+     306           0 :       break;
+     307             :     }
+     308          45 :     case MessageType_t::RANGE: {
+     309          45 :       sh_range_                   = mrs_lib::SubscribeHandler<sensor_msgs::Range>(shopts, msg_topic_, &Correction::callbackRange, this);
+     310          45 :       const std::size_t found     = ros::this_node::getName().find_last_of("/");
+     311          90 :       std::string       node_name = ros::this_node::getName().substr(found + 1);
+     312          90 :       ser_toggle_range_ =
+     313          45 :           nh.advertiseService(ros::this_node::getName() + "/" + getNamespacedName() + "/toggle_range_in", &Correction::callbackToggleRange, this);
+     314          45 :       break;
+     315             :     }
+     316           4 :     case MessageType_t::RTK_GPS: {
+     317           4 :       sh_rtk_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, msg_topic_, &Correction::callbackRtk, this);
+     318           4 :       break;
+     319             :     }
+     320          64 :     case MessageType_t::POINT: {
+     321          64 :       sh_point_ = mrs_lib::SubscribeHandler<geometry_msgs::PointStamped>(shopts, msg_topic_, &Correction::callbackPoint, this);
+     322          64 :       break;
+     323             :     }
+     324          41 :     case MessageType_t::VECTOR: {
+     325          41 :       sh_vector_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, msg_topic_, &Correction::callbackVector, this);
+     326          41 :       break;
+     327             :     }
+     328           0 :     case MessageType_t::QUAT: {
+     329           0 :       sh_quat_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, msg_topic_);
+     330           0 :       break;
+     331             :     }
+     332           0 :     case MessageType_t::UNKNOWN: {
+     333           0 :       ROS_ERROR("[%s]: UNKNOWN message type of correction", getPrintName().c_str());
+     334           0 :       break;
+     335             :     }
+     336           0 :     default: {
+     337           0 :       ROS_ERROR("[%s]: unhandled message type", getPrintName().c_str());
+     338             :     }
+     339             :   }
+     340             : 
+     341             :   // | ------ subscribe orientation for obtaingin hdg rate ------ |
+     342         154 :   if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) {
+     343           0 :     ph->param_loader->loadParam("message/orientation_topic", orientation_topic_);
+     344           0 :     orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_;
+     345           0 :     sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, orientation_topic_);
+     346             :   }
+     347             : 
+     348             : 
+     349             :   // | --------------- initialize publish handlers -------------- |
+     350         154 :   if (ch_->debug_topics.correction) {
+     351         154 :     ph_correction_raw_  = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/raw", 10);
+     352         154 :     ph_correction_proc_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/proc", 10);
+     353             :   }
+     354         154 :   if (ch_->debug_topics.corr_delay) {
+     355           0 :     ph_delay_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, est_name_ + "/correction/" + getName() + "/delay", 10);
+     356             :   }
+     357             : 
+     358             :   // | --- check whether all parameters were loaded correctly --- |
+     359         154 :   if (!ph->param_loader->loadedSuccessfully()) {
+     360           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     361           0 :     ros::shutdown();
+     362             :   }
+     363             : 
+     364         154 :   healthy_time_ = ros::Time(0);
+     365             : 
+     366         154 :   is_initialized_ = true;
+     367         154 : }
+     368             : /*//}*/
+     369             : 
+     370             : /*//{ getName() */
+     371             : template <int n_measurements>
+     372         308 : std::string Correction<n_measurements>::getName() const {
+     373         308 :   return name_;
+     374             : }
+     375             : /*//}*/
+     376             : 
+     377             : /*//{ getNamespacedName() */
+     378             : template <int n_measurements>
+     379         512 : std::string Correction<n_measurements>::getNamespacedName() const {
+     380         512 :   return est_name_ + "/" + name_;
+     381             : }
+     382             : /*//}*/
+     383             : 
+     384             : /*//{ getPrintName() */
+     385             : template <int n_measurements>
+     386         342 : std::string Correction<n_measurements>::getPrintName() const {
+     387         342 :   return ch_->nodelet_name + "/" + est_name_ + "/" + name_;
+     388             : }
+     389             : /*//}*/
+     390             : 
+     391             : /*//{ getR() */
+     392             : template <int n_measurements>
+     393     1362309 : double Correction<n_measurements>::getR() {
+     394     1362309 :   std::scoped_lock lock(mtx_R_);
+     395     1362159 :   default_R_ = drmgr_->config.noise;
+     396     2723942 :   return R_;
+     397             : }
+     398             : /*//}*/
+     399             : 
+     400             : /*//{ setR() */
+     401             : template <int n_measurements>
+     402      385397 : void Correction<n_measurements>::setR(const double R) {
+     403      385397 :   std::scoped_lock lock(mtx_R_);
+     404      385335 :   R_ = R;
+     405      385339 : }
+     406             : /*//}*/
+     407             : 
+     408             : /*//{ getStateId() */
+     409             : template <int n_measurements>
+     410      385480 : StateId_t Correction<n_measurements>::getStateId() const {
+     411      385480 :   return state_id_;
+     412             : }
+     413             : /*//}*/
+     414             : 
+     415             : /*//{ isHealthy() */
+     416             : template <int n_measurements>
+     417      188176 : bool Correction<n_measurements>::isHealthy() {
+     418             : 
+     419      188176 :   if (!is_initialized_) {
+     420           0 :     return false;
+     421             :   }
+     422             : 
+     423      188153 :   is_dt_ok_ = isMsgComing();
+     424             : 
+     425      188226 :   if (!is_delay_ok_) {
+     426           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: delay not ok", getPrintName().c_str());
+     427             :   }
+     428             : 
+     429      188161 :   if (!is_healthy_) {
+     430           0 :     if (is_dt_ok_ && is_delay_ok_) {
+     431           0 :       if (healthy_time_ > ros::Time(10)) {
+     432           0 :         is_healthy_ = true;
+     433             :       }
+     434             :     } else {
+     435           0 :       healthy_time_ = ros::Time(0);
+     436             :     }
+     437             :   }
+     438             : 
+     439      188149 :   is_healthy_ = is_healthy_ && is_dt_ok_ && is_delay_ok_;
+     440             : 
+     441      188132 :   return is_healthy_;
+     442             : }
+     443             : /*//}*/
+     444             : 
+     445             : /*//{ getRawCorrection() */
+     446             : template <int n_measurements>
+     447        3960 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getRawCorrection() {
+     448             : 
+     449        3960 :   if (!is_initialized_) {
+     450           0 :     return {};
+     451             :   }
+     452             : 
+     453        3957 :   MeasurementStamped measurement_stamped;
+     454             : 
+     455        3954 :   switch (msg_type_) {
+     456             : 
+     457           0 :     case MessageType_t::ODOMETRY: {
+     458             : 
+     459           0 :       if (!sh_odom_.hasMsg()) {
+     460           0 :         return {};
+     461             :       }
+     462             : 
+     463           0 :       auto msg                  = sh_odom_.getMsg();
+     464           0 :       measurement_stamped.stamp = msg->header.stamp;
+     465             :       /* if (!isTimestampOk(measurement_stamped.stamp)) { */
+     466             :       /*   return {}; */
+     467             :       /* } */
+     468             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     469             : 
+     470             :       /* if (!is_delay_ok_) { */
+     471             :       /*   return {}; */
+     472             :       /* } */
+     473           0 :       auto res = getCorrectionFromOdometry(msg);
+     474           0 :       if (res) {
+     475           0 :         measurement_stamped.value = res.value();
+     476             :       } else {
+     477           0 :         return {};
+     478             :       }
+     479           0 :       break;
+     480             :     }
+     481             : 
+     482           0 :     case MessageType_t::POSE: {
+     483             : 
+     484           0 :       if (!sh_pose_s_.hasMsg()) {
+     485           0 :         return {};
+     486             :       }
+     487             : 
+     488           0 :       auto msg                  = sh_pose_s_.getMsg();
+     489           0 :       measurement_stamped.stamp = msg->header.stamp;
+     490             :       /* if (!isTimestampOk(measurement_stamped.stamp)) { */
+     491             :       /*   return {}; */
+     492             :       /* } */
+     493             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     494             : 
+     495             :       /* if (!is_delay_ok_) { */
+     496             :       /*   return {}; */
+     497             :       /* } */
+     498           0 :       auto res = getCorrectionFromPoseStamped(msg);
+     499           0 :       if (res) {
+     500           0 :         measurement_stamped.value = res.value();
+     501             :       } else {
+     502           0 :         return {};
+     503             :       }
+     504           0 :       break;
+     505             :     }
+     506             : 
+     507           0 :     case MessageType_t::POSECOV: {
+     508             :       // TODO implement
+     509             :       /* return getCorrectionFromPoseWCS(msg); */
+     510           0 :       is_healthy_ = false;
+     511           0 :       return {};
+     512             :       break;
+     513             :     }
+     514             : 
+     515        2130 :     case MessageType_t::RANGE: {
+     516             : 
+     517        2130 :       if (!range_enabled_) {
+     518           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+     519        1600 :         return {};
+     520             :       }
+     521             : 
+     522        2130 :       if (!sh_range_.hasMsg()) {
+     523         638 :         return {};
+     524             :       }
+     525             : 
+     526        1491 :       auto msg                  = sh_range_.getMsg();
+     527        1490 :       measurement_stamped.stamp = msg->header.stamp;
+     528             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     529             : 
+     530             :       /* if (!is_delay_ok_) { */
+     531             :       /*   return {}; */
+     532             :       /* } */
+     533             : 
+     534        1492 :       auto res = getCorrectionFromRange(msg);
+     535        1492 :       if (res) {
+     536         530 :         measurement_stamped.value = res.value();
+     537             :       } else {
+     538         962 :         return {};
+     539             :       }
+     540         530 :       break;
+     541             :     }
+     542             : 
+     543          83 :     case MessageType_t::RTK_GPS: {
+     544             : 
+     545          83 :       if (!sh_rtk_.hasMsg()) {
+     546           0 :         ROS_ERROR_THROTTLE(1.0, " no rtk msgs so far");
+     547          13 :         return {};
+     548             :       }
+     549             : 
+     550          83 :       auto msg                  = sh_rtk_.getMsg();
+     551          83 :       measurement_stamped.stamp = msg->header.stamp;
+     552             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     553             : 
+     554             :       /* if (!is_delay_ok_) { */
+     555             :       /*   ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */
+     556             :       /*   return {}; */
+     557             :       /* } */
+     558             : 
+     559          83 :       auto res = getCorrectionFromRtk(msg);
+     560          83 :       if (res) {
+     561          70 :         measurement_stamped.value = res.value();
+     562             :       } else {
+     563          13 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not get rtk correction", ros::this_node::getName().c_str());
+     564          13 :         return {};
+     565             :       }
+     566             : 
+     567          70 :       break;
+     568             :     }
+     569             : 
+     570        1570 :     case MessageType_t::POINT: {
+     571             : 
+     572        1570 :       if (!sh_point_.hasMsg()) {
+     573        1436 :         return {};
+     574             :       }
+     575             : 
+     576         134 :       auto msg                  = sh_point_.getMsg();
+     577         134 :       measurement_stamped.stamp = msg->header.stamp;
+     578             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     579             : 
+     580             :       /* if (!is_delay_ok_) { */
+     581             :       /*   return {}; */
+     582             :       /* } */
+     583         134 :       auto res = getCorrectionFromPoint(msg);
+     584         134 :       if (res) {
+     585         134 :         measurement_stamped.value = res.value();
+     586             :       } else {
+     587           0 :         return {};
+     588             :       }
+     589         134 :       break;
+     590             :     }
+     591             : 
+     592         177 :     case MessageType_t::VECTOR: {
+     593             : 
+     594         177 :       if (!sh_vector_.hasMsg()) {
+     595         136 :         return {};
+     596             :       }
+     597             : 
+     598          43 :       auto msg                  = sh_vector_.getMsg();
+     599          43 :       measurement_stamped.stamp = msg->header.stamp;
+     600             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     601             : 
+     602             :       /* if (!is_delay_ok_) { */
+     603             :       /*   return {}; */
+     604             :       /* } */
+     605          43 :       auto res = getCorrectionFromVector(msg);
+     606          43 :       if (res) {
+     607          41 :         measurement_stamped.value = res.value();
+     608             :       } else {
+     609           2 :         return {};
+     610             :       }
+     611          41 :       break;
+     612             :     }
+     613             : 
+     614           0 :     case MessageType_t::QUAT: {
+     615             : 
+     616           0 :       if (!sh_quat_.newMsg()) {
+     617           0 :         return {};
+     618             :       }
+     619             : 
+     620           0 :       auto msg                  = sh_quat_.getMsg();
+     621           0 :       measurement_stamped.stamp = msg->header.stamp;
+     622             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     623             : 
+     624             :       /* if (!is_delay_ok_) { */
+     625             :       /*   return {}; */
+     626             :       /* } */
+     627           0 :       auto res = getCorrectionFromQuat(msg);
+     628           0 :       if (res) {
+     629           0 :         measurement_stamped.value = res.value();
+     630             :       } else {
+     631           0 :         return {};
+     632             :       }
+     633           0 :       break;
+     634             :     }
+     635             : 
+     636           0 :     default: {
+     637           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: this type of correction is not implemented in getCorrectionFromMessage()", getPrintName().c_str());
+     638           0 :       is_healthy_ = false;
+     639           0 :       return {};
+     640             :     }
+     641             :   }
+     642             : 
+     643             :   // check for nans
+     644         775 :   is_nan_free_ = true;
+     645        1635 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+     646         860 :     if (!std::isfinite(measurement_stamped.value(i))) {
+     647           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in correction. Total NaNs: %d", getPrintName().c_str(), ++counter_nan_);
+     648           0 :       is_nan_free_ = false;
+     649           0 :       return {};
+     650             :     }
+     651             :   }
+     652             : 
+     653         775 :   got_first_msg_ = true;
+     654         775 :   publishCorrection(measurement_stamped, ph_correction_raw_);
+     655             : 
+     656         775 :   return measurement_stamped;
+     657             : }
+     658             : /*//}*/
+     659             : 
+     660             : /*//{ getProcessedCorrection() */
+     661             : template <int n_measurements>
+     662        3959 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getProcessedCorrection() {
+     663             : 
+     664        3959 :   MeasurementStamped measurement_stamped;
+     665        3958 :   auto               res = getRawCorrection();
+     666        3960 :   if (res) {
+     667         775 :     MeasurementStamped measurement_stamped = res.value();
+     668         775 :     if (process(measurement_stamped.value)) {
+     669         290 :       publishCorrection(measurement_stamped, ph_correction_proc_);
+     670         290 :       return measurement_stamped;
+     671             :     } else {
+     672         485 :       return {};  // invalid correction
+     673             :     }
+     674             :   } else {
+     675        3185 :     return {};  // invalid correction
+     676             :   }
+     677             : }  // namespace mrs_uav_state_estimation
+     678             : /*//}*/
+     679             : 
+     680             : /*//{ callbackOdometry() */
+     681             : template <int n_measurements>
+     682           0 : void Correction<n_measurements>::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+     683             : 
+     684           0 :   if (!is_initialized_) {
+     685           0 :     return;
+     686             :   }
+     687             : 
+     688           0 :   auto res = getCorrectionFromOdometry(msg);
+     689           0 :   if (res) {
+     690           0 :     applyCorrection(res.value(), msg->header.stamp);
+     691             :   }
+     692             : }
+     693             : /*//}*/
+     694             : 
+     695             : /*//{ getCorrectionFromOdometry() */
+     696             : template <int n_measurements>
+     697           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg) {
+     698             : 
+     699           0 :   switch (est_type_) {
+     700             : 
+     701             :     // handle lateral estimators
+     702           0 :     case EstimatorType_t::LATERAL: {
+     703             : 
+     704           0 :       switch (state_id_) {
+     705             : 
+     706           0 :         case StateId_t::POSITION: {
+     707           0 :           measurement_t measurement;
+     708           0 :           measurement(0) = msg->pose.pose.position.x;
+     709           0 :           measurement(1) = msg->pose.pose.position.y;
+     710           0 :           return measurement;
+     711             :           break;
+     712             :         }
+     713             : 
+     714           0 :         case StateId_t::VELOCITY: {
+     715           0 :           if (is_in_body_frame_) {
+     716           0 :             std_msgs::Header header = msg->header;
+     717           0 :             header.frame_id         = ch_->frames.ns_fcu;  // message in odometry is published in body frame
+     718           0 :             auto res                = getVelInFrame(msg->twist.twist.linear, header, ns_frame_id_ + "_att_only");
+     719           0 :             if (res) {
+     720           0 :               measurement_t measurement;
+     721           0 :               measurement = res.value();
+     722           0 :               return measurement;
+     723             :             } else {
+     724           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+     725           0 :               return {};
+     726             :             }
+     727             :           } else {
+     728           0 :             measurement_t measurement;
+     729           0 :             measurement(0) = msg->twist.twist.linear.x;
+     730           0 :             measurement(1) = msg->twist.twist.linear.y;
+     731           0 :             return measurement;
+     732             :           }
+     733             :           break;
+     734             :         }
+     735             : 
+     736           0 :         default: {
+     737           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     738           0 :           return {};
+     739             :         }
+     740             :       }
+     741             :       break;
+     742             :     }
+     743             : 
+     744             :     // handle altitude estimators
+     745           0 :     case EstimatorType_t::ALTITUDE: {
+     746             : 
+     747           0 :       switch (state_id_) {
+     748             : 
+     749           0 :         case StateId_t::POSITION: {
+     750           0 :           measurement_t measurement;
+     751           0 :           measurement(0) = msg->pose.pose.position.z;
+     752           0 :           return measurement;
+     753             :           break;
+     754             :         }
+     755             : 
+     756           0 :         case StateId_t::VELOCITY: {
+     757           0 :           if (is_in_body_frame_) {
+     758           0 :             std_msgs::Header header = msg->header;
+     759           0 :             header.frame_id         = ch_->frames.ns_fcu;
+     760           0 :             auto res                = getZVelUntilted(msg->twist.twist.linear, header);
+     761           0 :             if (res) {
+     762           0 :               measurement_t measurement;
+     763           0 :               measurement = res.value();
+     764           0 :               return measurement;
+     765             :             } else {
+     766           0 :               return {};
+     767             :             }
+     768             :           } else {
+     769           0 :             measurement_t measurement;
+     770           0 :             measurement(0) = msg->twist.twist.linear.z;
+     771           0 :             return measurement;
+     772             :           }
+     773             :           break;
+     774             :         }
+     775             : 
+     776           0 :         default: {
+     777           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     778           0 :           return {};
+     779             :         }
+     780             :       }
+     781             :       break;
+     782             :     }
+     783             : 
+     784             :     // handle heading estimators
+     785           0 :     case EstimatorType_t::HEADING: {
+     786             : 
+     787           0 :       switch (state_id_) {
+     788             : 
+     789           0 :         case StateId_t::POSITION: {
+     790           0 :           measurement_t measurement;
+     791             :           try {
+     792             :             // obtain heading from orientation
+     793           0 :             measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading();
+     794             :             // unwrap heading wrt previous measurement
+     795           0 :             if (got_first_hdg_measurement_) {
+     796           0 :               measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+     797             :             } else {
+     798           0 :               got_first_hdg_measurement_ = true;
+     799             :             }
+     800           0 :             prev_hdg_measurement_ = measurement;
+     801           0 :             return measurement;
+     802             :           }
+     803           0 :           catch (...) {
+     804           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: failed to obtain heading", getPrintName().c_str());
+     805           0 :             return {};
+     806             :           }
+     807             :           break;
+     808             :         }
+     809             : 
+     810             :           /* case StateId_t::VELOCITY: { */
+     811             :           /*   try { */
+     812             :           /*     measurement_t measurement; */
+     813             :           /*     measurement(0) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeadingRate(msg->twist.twist.angular); */
+     814             :           /*     return measurement; */
+     815             :           /*   } */
+     816             :           /*   catch (...) { */
+     817             :           /*     ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromOdometry())", getPrintName().c_str()); */
+     818             :           /*     return {}; */
+     819             :           /*   } */
+     820             :           /*   break; */
+     821             :           /* } */
+     822             : 
+     823           0 :         default: {
+     824           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     825           0 :           return {};
+     826             :         }
+     827             :       }
+     828             :       break;
+     829             :     }
+     830             :   }
+     831             : 
+     832           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+     833           0 :   return {};
+     834             : }
+     835             : /*//}*/
+     836             : 
+     837             : /*//{ callbackPoseStamped() */
+     838             : template <int n_measurements>
+     839           0 : void Correction<n_measurements>::callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg) {
+     840             : 
+     841           0 :   if (!is_initialized_) {
+     842           0 :     return;
+     843             :   }
+     844             : 
+     845           0 :   auto res = getCorrectionFromPoseStamped(msg);
+     846           0 :   if (res) {
+     847           0 :     applyCorrection(res.value(), msg->header.stamp);
+     848             :   }
+     849             : }
+     850             : /*//}*/
+     851             : 
+     852             : /*//{ getCorrectionFromPoseStamped() */
+     853             : template <int n_measurements>
+     854           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoseStamped(
+     855             :     const geometry_msgs::PoseStampedConstPtr msg) {
+     856             : 
+     857           0 :   switch (est_type_) {
+     858             : 
+     859             :     // handle lateral estimators
+     860           0 :     case EstimatorType_t::LATERAL: {
+     861             : 
+     862           0 :       switch (state_id_) {
+     863             : 
+     864           0 :         case StateId_t::POSITION: {
+     865           0 :           measurement_t measurement;
+     866           0 :           measurement(0) = msg->pose.position.x;
+     867           0 :           measurement(1) = msg->pose.position.y;
+     868           0 :           return measurement;
+     869             :           break;
+     870             :         }
+     871             : 
+     872           0 :         default: {
+     873           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+     874           0 :           return {};
+     875             :         }
+     876             :       }
+     877             :       break;
+     878             :     }
+     879             : 
+     880             :     // handle altitude estimators
+     881           0 :     case EstimatorType_t::ALTITUDE: {
+     882             : 
+     883           0 :       switch (state_id_) {
+     884             : 
+     885           0 :         case StateId_t::POSITION: {
+     886           0 :           measurement_t measurement;
+     887           0 :           measurement(0) = msg->pose.position.z;
+     888           0 :           return measurement;
+     889             :           break;
+     890             :         }
+     891             : 
+     892           0 :         default: {
+     893           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+     894           0 :           return {};
+     895             :         }
+     896             :       }
+     897             :       break;
+     898             :     }
+     899             : 
+     900             :     // handle heading estimators
+     901           0 :     case EstimatorType_t::HEADING: {
+     902             : 
+     903           0 :       switch (state_id_) {
+     904             : 
+     905           0 :         case StateId_t::POSITION: {
+     906           0 :           measurement_t measurement;
+     907             :           try {
+     908             :             // obtain heading from orientation
+     909           0 :             measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.orientation).getHeading();
+     910             :             // unwrap heading wrt previous measurement
+     911           0 :             if (got_first_hdg_measurement_) {
+     912           0 :               measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+     913             :             } else {
+     914           0 :               got_first_hdg_measurement_ = true;
+     915             :             }
+     916           0 :             prev_hdg_measurement_ = measurement;
+     917           0 :             return measurement;
+     918             :           }
+     919           0 :           catch (...) {
+     920           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: failed to obtain heading", getPrintName().c_str());
+     921           0 :             return {};
+     922             :           }
+     923             :           break;
+     924             :         }
+     925             : 
+     926           0 :         default: {
+     927           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+     928           0 :           return {};
+     929             :         }
+     930             :       }
+     931             :       break;
+     932             :     }
+     933             :   }
+     934             : 
+     935           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+     936           0 :   return {};
+     937             : }
+     938             : /*//}*/
+     939             : 
+     940             : /*//{ callbackRange() */
+     941             : template <int n_measurements>
+     942       99665 : void Correction<n_measurements>::callbackRange(const sensor_msgs::Range::ConstPtr msg) {
+     943             : 
+     944       99665 :   if (!is_initialized_) {
+     945           0 :     return;
+     946             :   }
+     947             : 
+     948       99551 :   auto res = getCorrectionFromRange(msg);
+     949       99762 :   if (res) {
+     950       98952 :     applyCorrection(res.value(), msg->header.stamp);
+     951             :   }
+     952             : }
+     953             : /*//}*/
+     954             : 
+     955             : /*//{ getCorrectionFromRange() */
+     956             : template <int n_measurements>
+     957      101011 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg) {
+     958             : 
+     959      101011 :   if (!range_enabled_) {
+     960           0 :     ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+     961           0 :     return {};
+     962             :   }
+     963             : 
+     964      101011 :   if (!std::isfinite(msg->range)) {
+     965          10 :     ROS_ERROR_THROTTLE(1.0, "[%s]: received value: %f. Not using this correction.", getPrintName().c_str(), msg->range);
+     966          10 :     return {};
+     967             :   }
+     968             : 
+     969      202077 :   geometry_msgs::PoseStamped range_point;
+     970             : 
+     971      100898 :   range_point.header           = msg->header;
+     972      101087 :   range_point.pose.position.x  = msg->range;
+     973      100744 :   range_point.pose.position.y  = 0;
+     974      100744 :   range_point.pose.position.z  = 0;
+     975      100744 :   range_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     976             : 
+     977      202229 :   auto res = ch_->transformer->transformSingle(range_point, ch_->frames.ns_fcu_untilted);
+     978             : 
+     979      101245 :   Correction::measurement_t measurement;
+     980             : 
+     981      101245 :   if (res) {
+     982       99483 :     measurement(0) = -res.value().pose.position.z;
+     983       99482 :     return measurement;
+     984             :   } else {
+     985        1762 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform range measurement to %s. Not using this correction.", getPrintName().c_str(),
+     986             :                        ch_->frames.ns_fcu_untilted.c_str());
+     987        1762 :     return {};
+     988             :   }
+     989             : }
+     990             : /*//}*/
+     991             : 
+     992             : /*//{ callbackRtk() */
+     993             : template <int n_measurements>
+     994        1991 : void Correction<n_measurements>::callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg) {
+     995             : 
+     996        1991 :   if (!is_initialized_) {
+     997           0 :     return;
+     998             :   }
+     999             : 
+    1000        1991 :   auto res = getCorrectionFromRtk(msg);
+    1001        1991 :   if (res) {
+    1002        1982 :     applyCorrection(res.value(), msg->header.stamp);
+    1003             :   }
+    1004             : }
+    1005             : /*//}*/
+    1006             : 
+    1007             : /*//{ getCorrectionFromRtk() */
+    1008             : template <int n_measurements>
+    1009        2074 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg) {
+    1010             : 
+    1011        4148 :   geometry_msgs::PoseStamped rtk_pos;
+    1012             : 
+    1013        2074 :   if (!std::isfinite(msg->gps.latitude)) {
+    1014           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+    1015           0 :     return {};
+    1016             :   }
+    1017             : 
+    1018        2074 :   if (!std::isfinite(msg->gps.longitude)) {
+    1019           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+    1020           0 :     return {};
+    1021             :   }
+    1022             : 
+    1023        2074 :   if (!std::isfinite(msg->gps.altitude)) {
+    1024           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+    1025           0 :     return {};
+    1026             :   }
+    1027             : 
+    1028        2074 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+    1029           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+    1030           0 :     return {};
+    1031             :   }
+    1032             : 
+    1033        2074 :   rtk_pos.header = msg->header;
+    1034        2074 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+    1035        2074 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+    1036        2074 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1037             : 
+    1038        2074 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+    1039        2074 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+    1040             : 
+    1041        2074 :   Correction::measurement_t measurement;
+    1042             : 
+    1043             :   // transform the RTK position from antenna to FCU
+    1044        2074 :   auto res = transformRtkToFcu(rtk_pos);
+    1045        2074 :   if (res) {
+    1046        2074 :     rtk_pos.pose = res.value();
+    1047             :   } else {
+    1048           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+    1049           0 :     return {};
+    1050             :   }
+    1051             : 
+    1052        2074 :   switch (est_type_) {
+    1053             : 
+    1054             :     // handle lateral estimators
+    1055        1043 :     case EstimatorType_t::LATERAL: {
+    1056             : 
+    1057        1043 :       switch (state_id_) {
+    1058             : 
+    1059        1043 :         case StateId_t::POSITION: {
+    1060        1043 :           measurement(0) = rtk_pos.pose.position.x;
+    1061        1043 :           measurement(1) = rtk_pos.pose.position.y;
+    1062        1043 :           return measurement;
+    1063             :           break;
+    1064             :         }
+    1065             : 
+    1066           0 :         default: {
+    1067           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1068           0 :           return {};
+    1069             :         }
+    1070             :       }
+    1071             :       break;
+    1072             :     }
+    1073             : 
+    1074             :     // handle altitude estimators
+    1075        1031 :     case EstimatorType_t::ALTITUDE: {
+    1076             : 
+    1077        1031 :       switch (state_id_) {
+    1078             : 
+    1079        1031 :         case StateId_t::POSITION: {
+    1080        1031 :           measurement(0) = rtk_pos.pose.position.z;
+    1081        1031 :           if (!got_avg_init_rtk_z_) {
+    1082          22 :             getAvgRtkInitZ(measurement(0));
+    1083          22 :             return {};
+    1084             :           }
+    1085        1009 :           measurement(0) -= rtk_init_z_avg_;
+    1086        1009 :           return measurement;
+    1087             :           break;
+    1088             :         }
+    1089             : 
+    1090           0 :         default: {
+    1091           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1092           0 :           return {};
+    1093             :         }
+    1094             :       }
+    1095             :       break;
+    1096             :     }
+    1097             : 
+    1098           0 :     case EstimatorType_t::HEADING: {
+    1099           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: should not be possible to get into this branch of getCorrectionFromRtk() switch", getPrintName().c_str());
+    1100           0 :       return {};
+    1101             :       break;
+    1102             :     }
+    1103             :   }
+    1104             : 
+    1105           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1106           0 :   return {};
+    1107             : }
+    1108             : /*//}*/
+    1109             : 
+    1110             : /*//{ callbackPoint() */
+    1111             : template <int n_measurements>
+    1112      180575 : void Correction<n_measurements>::callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg) {
+    1113             : 
+    1114      180575 :   if (!is_initialized_) {
+    1115           0 :     return;
+    1116             :   }
+    1117             : 
+    1118      180575 :   auto res = getCorrectionFromPoint(msg);
+    1119      180575 :   if (res) {
+    1120      180575 :     applyCorrection(res.value(), msg->header.stamp);
+    1121             :   }
+    1122             : }
+    1123             : /*//}*/
+    1124             : 
+    1125             : /*//{ getCorrectionFromPoint() */
+    1126             : template <int n_measurements>
+    1127      180709 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoint(
+    1128             :     const geometry_msgs::PointStampedConstPtr msg) {
+    1129             : 
+    1130      180709 :   switch (est_type_) {
+    1131             : 
+    1132             :     // handle lateral estimators
+    1133       97272 :     case EstimatorType_t::LATERAL: {
+    1134             : 
+    1135       97272 :       switch (state_id_) {
+    1136             : 
+    1137       97272 :         case StateId_t::POSITION: {
+    1138       97272 :           measurement_t measurement;
+    1139       97272 :           measurement(0) = msg->point.x;
+    1140       97272 :           measurement(1) = msg->point.y;
+    1141       97272 :           return measurement;
+    1142             :           break;
+    1143             :         }
+    1144             : 
+    1145           0 :         default: {
+    1146           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1147           0 :           return {};
+    1148             :         }
+    1149             :       }
+    1150             :       break;
+    1151             :     }
+    1152             : 
+    1153             :     // handle altitude estimators
+    1154       83437 :     case EstimatorType_t::ALTITUDE: {
+    1155             : 
+    1156       83437 :       switch (state_id_) {
+    1157             : 
+    1158       83437 :         case StateId_t::POSITION: {
+    1159       83437 :           measurement_t measurement;
+    1160       83437 :           measurement(0) = msg->point.z;
+    1161       83437 :           return measurement;
+    1162             :           break;
+    1163             :         }
+    1164             : 
+    1165           0 :         default: {
+    1166           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1167           0 :           return {};
+    1168             :         }
+    1169             :       }
+    1170             :       break;
+    1171             :     }
+    1172             : 
+    1173           0 :     default: {
+    1174           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1175           0 :       return {};
+    1176             :     }
+    1177             :   }
+    1178             : 
+    1179             : 
+    1180             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1181             :   return {};
+    1182             : }
+    1183             : /*//}*/
+    1184             : 
+    1185             : /*//{ callbackVector() */
+    1186             : template <int n_measurements>
+    1187      104117 : void Correction<n_measurements>::callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+    1188             : 
+    1189      104117 :   if (!is_initialized_) {
+    1190           0 :     return;
+    1191             :   }
+    1192             : 
+    1193      104117 :   auto res = getCorrectionFromVector(msg);
+    1194      104117 :   if (res) {
+    1195      104110 :     applyCorrection(res.value(), msg->header.stamp);
+    1196             :   }
+    1197             : }
+    1198             : /*//}*/
+    1199             : 
+    1200             : /*//{ getCorrectionFromVector() */
+    1201             : template <int n_measurements>
+    1202      104160 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromVector(
+    1203             :     const geometry_msgs::Vector3StampedConstPtr msg) {
+    1204             : 
+    1205      104160 :   switch (est_type_) {
+    1206             : 
+    1207             :     // handle lateral estimators
+    1208        4470 :     case EstimatorType_t::LATERAL: {
+    1209             : 
+    1210        4470 :       switch (state_id_) {
+    1211             : 
+    1212        4470 :         case StateId_t::VELOCITY: {
+    1213        4470 :           auto res = getVelInFrame(msg->vector, msg->header, ns_frame_id_ + "_att_only");
+    1214        4470 :           if (res) {
+    1215        4462 :             measurement_t measurement;
+    1216        4462 :             measurement = res.value();
+    1217        4462 :             return measurement;
+    1218             :           } else {
+    1219           8 :             return {};
+    1220             :           }
+    1221             :           break;
+    1222             :         }
+    1223             : 
+    1224           0 :         default: {
+    1225           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1226           0 :           return {};
+    1227             :         }
+    1228             :       }
+    1229             :       break;
+    1230             :     }
+    1231             : 
+    1232             :     // handle altitude estimators
+    1233       99690 :     case EstimatorType_t::ALTITUDE: {
+    1234             : 
+    1235       99690 :       switch (state_id_) {
+    1236             : 
+    1237       99690 :         case StateId_t::VELOCITY: {
+    1238       99690 :           auto res = getZVelUntilted(msg->vector, msg->header);
+    1239       99690 :           if (res) {
+    1240       99689 :             measurement_t measurement;
+    1241       99689 :             measurement = res.value();
+    1242       99689 :             return measurement;
+    1243             :           } else {
+    1244           1 :             return {};
+    1245             :           }
+    1246             :           break;
+    1247             :         }
+    1248             : 
+    1249           0 :         default: {
+    1250           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1251           0 :           return {};
+    1252             :         }
+    1253             :       }
+    1254             :       break;
+    1255             :     }
+    1256             : 
+    1257             :     // handle heading estimators
+    1258           0 :     case EstimatorType_t::HEADING: {
+    1259             : 
+    1260           0 :       switch (state_id_) {
+    1261             : 
+    1262           0 :           case StateId_t::VELOCITY: {
+    1263             :             try {
+    1264           0 :               if (!sh_orientation_.hasMsg()) {
+    1265           0 :               ROS_INFO_THROTTLE(1.0, "[%s]: %s orientation on topic: %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), orientation_topic_.c_str());
+    1266           0 :                 return {};
+    1267             :               }
+    1268           0 :               measurement_t measurement;
+    1269           0 :               measurement(0) = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+    1270           0 :               return measurement;
+    1271             :             }
+    1272           0 :             catch (...) {
+    1273           0 :               ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromVector())", getPrintName().c_str());
+    1274           0 :               return {};
+    1275             :             }
+    1276             :             break;
+    1277             :           }
+    1278             : 
+    1279           0 :         default: {
+    1280           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    1281           0 :           return {};
+    1282             :         }
+    1283             :       }
+    1284             :       break;
+    1285             :     }
+    1286             :   }
+    1287             : 
+    1288           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1289           0 :   return {};
+    1290             : }
+    1291             : /*//}*/
+    1292             : 
+    1293             : /*//{ getCorrectionFromQuat() */
+    1294             : template <int n_measurements>
+    1295           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromQuat(
+    1296             :     const geometry_msgs::QuaternionStampedConstPtr msg) {
+    1297             : 
+    1298           0 :   switch (est_type_) {
+    1299             : 
+    1300             :       // handle lateral estimators
+    1301             :       /* case EstimatorType_t::LATERAL: { */
+    1302             : 
+    1303             :       /*   default: { */
+    1304             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1305             :       /*     return {}; */
+    1306             :       /*   } break; */
+    1307             :       /* } */
+    1308             : 
+    1309             :       /* // handle altitude estimators */
+    1310             :       /* case EstimatorType_t::ALTITUDE: { */
+    1311             : 
+    1312             :       /* default: { */
+    1313             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1314             :       /*     return {}; */
+    1315             :       /*   } break; */
+    1316             :       /* } */
+    1317             : 
+    1318             :       /* // handle heading estimators */
+    1319             :       /* case EstimatorType_t::HEADING: { */
+    1320             : 
+    1321             :       /*   switch (state_id_) { */
+    1322             : 
+    1323             :       /*     /1* default: { *1/ */
+    1324             :       /*       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    1325             :       /*       return {}; */
+    1326             :       /*     } */
+    1327             :       /*   } */
+    1328             :     default: {
+    1329           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1330           0 :       return {};
+    1331             :     }
+    1332             :   }
+    1333             : 
+    1334             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1335             :   return {};
+    1336             : }
+    1337             : /*//}*/
+    1338             : 
+    1339             : /*//{ applyCorrection() */
+    1340             : template <int n_measurements>
+    1341      385612 : void Correction<n_measurements>::applyCorrection(const measurement_t& meas, const ros::Time& stamp) {
+    1342             : 
+    1343             :   {
+    1344      385612 :     std::scoped_lock lock(mtx_msg_time_);
+    1345      385601 :     if (first_timestamp_) {
+    1346         154 :       prev_msg_time_   = stamp - ros::Duration(0.01);
+    1347         154 :       msg_time_        = stamp;
+    1348         154 :       healthy_time_    = ros::Time(0);
+    1349         154 :       first_timestamp_ = false;
+    1350             :     }
+    1351             : 
+    1352      385603 :     prev_msg_time_ = msg_time_;
+    1353      385603 :     msg_time_      = stamp;
+    1354      385603 :     healthy_time_ += msg_time_ - prev_msg_time_;
+    1355             :   }
+    1356             : 
+    1357      385573 :   MeasurementStamped meas_stamped;
+    1358      385543 :   meas_stamped.value = meas;
+    1359      385556 :   meas_stamped.stamp = stamp;
+    1360      385556 :   publishCorrection(meas_stamped, ph_correction_raw_);
+    1361      385610 :   if (process(meas_stamped.value)) {
+    1362      385019 :     publishCorrection(meas_stamped, ph_correction_proc_);
+    1363      385112 :     fun_apply_correction_(meas_stamped, getR(), getStateId());
+    1364             :   }
+    1365      385434 : }
+    1366             : /*//}*/
+    1367             : 
+    1368             : /* //{ callbackToggleRange() */
+    1369             : template <int n_measurements>
+    1370           0 : bool Correction<n_measurements>::callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1371             : 
+    1372           0 :   if (!is_initialized_) {
+    1373           0 :     return false;
+    1374             :   }
+    1375             : 
+    1376           0 :   if (!range_enabled_ && req.data) {
+    1377           0 :     processors_["saturate"]->toggle(true);
+    1378             :   }
+    1379             : 
+    1380           0 :   range_enabled_ = req.data;
+    1381             : 
+    1382             :   // after enabling range we want to start correcting the altitude slowly
+    1383             : 
+    1384           0 :   res.success = true;
+    1385           0 :   res.message = (range_enabled_ ? "Range enabled" : "Range disabled");
+    1386             : 
+    1387           0 :   if (range_enabled_) {
+    1388             : 
+    1389           0 :     ROS_INFO("[%s]: Range enabled.", getPrintName().c_str());
+    1390             : 
+    1391             :   } else {
+    1392             : 
+    1393           0 :     ROS_INFO("[%s]: Range disabled", getPrintName().c_str());
+    1394             :   }
+    1395             : 
+    1396           0 :   return true;
+    1397             : }
+    1398             : 
+    1399             : //}
+    1400             : 
+    1401             : /*//{ getZVelUntilted() */
+    1402             : template <int n_measurements>
+    1403       99690 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getZVelUntilted(const geometry_msgs::Vector3& msg,
+    1404             :                                                                                                               const std_msgs::Header&       header) {
+    1405             : 
+    1406             :   // untilt the desired vector
+    1407      199380 :   geometry_msgs::PointStamped vel;
+    1408       99690 :   vel.point.x = msg.x;
+    1409       99690 :   vel.point.y = msg.y;
+    1410       99690 :   vel.point.z = msg.z;
+    1411       99690 :   vel.header  = header;
+    1412             :   /* vel.header.frame_id = ch_->frames.ns_fcu; */
+    1413       99690 :   vel.header.stamp = header.stamp;
+    1414             : 
+    1415      199380 :   auto res = ch_->transformer->transformSingle(vel, ch_->frames.ns_fcu_untilted);
+    1416       99690 :   if (res) {
+    1417       99689 :     measurement_t measurement;
+    1418       99689 :     measurement(0) = res.value().point.z;
+    1419       99689 :     return measurement;
+    1420             :   } else {
+    1421           1 :     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());
+    1422           1 :     return {};
+    1423             :   }
+    1424             : }
+    1425             : /*//}*/
+    1426             : 
+    1427             : /*//{ getVelInFrame() */
+    1428             : template <int n_measurements>
+    1429        4470 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getVelInFrame(const geometry_msgs::Vector3& vel_in,
+    1430             :                                                                                                             const std_msgs::Header&       source_header,
+    1431             :                                                                                                             const std::string             target_frame) {
+    1432             : 
+    1433        4470 :   measurement_t measurement;
+    1434             : 
+    1435        8940 :   geometry_msgs::Vector3Stamped vel;
+    1436        4470 :   vel.header = source_header;
+    1437        4470 :   vel.vector = vel_in;
+    1438             :   /* body.vector.x = vel.x; */
+    1439             :   /* body.vector.y = vel.y; */
+    1440             :   /* body.vector.z = vel.z; */
+    1441             : 
+    1442        8940 :   geometry_msgs::Vector3Stamped transformed_vel;
+    1443        8940 :   auto                          res = ch_->transformer->transformSingle(vel, target_frame);
+    1444        4470 :   if (res) {
+    1445        4462 :     transformed_vel = res.value();
+    1446        4462 :     measurement(0)  = transformed_vel.vector.x;
+    1447        4462 :     measurement(1)  = transformed_vel.vector.y;
+    1448        4462 :     return measurement;
+    1449             :   } else {
+    1450           8 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform of velocity from %s to %s failed.", getPrintName().c_str(), vel.header.frame_id.c_str(), target_frame.c_str());
+    1451           8 :     return {};
+    1452             :   }
+    1453             : }
+    1454             : /*//}*/
+    1455             : 
+    1456             : /*//{ transformRtkToFcu() */
+    1457             : template <int n_measurements>
+    1458        2074 : std::optional<geometry_msgs::Pose> Correction<n_measurements>::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+    1459             : 
+    1460        4148 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+    1461             : 
+    1462             :   // inject current orientation into rtk pose
+    1463        4148 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+    1464        2074 :   if (res1) {
+    1465        2074 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+    1466             :   } else {
+    1467           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(),
+    1468             :                        ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str());
+    1469           0 :     return {};
+    1470             :   }
+    1471             : 
+    1472             :   // invert tf
+    1473        2074 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+    1474        4148 :   geometry_msgs::PoseStamped utm_in_antenna;
+    1475        2074 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+    1476        2074 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+    1477        2074 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+    1478             : 
+    1479             :   // transform to fcu
+    1480        4148 :   geometry_msgs::PoseStamped utm_in_fcu;
+    1481        2074 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+    1482        2074 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+    1483        4148 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+    1484             : 
+    1485        2074 :   if (res2) {
+    1486        2074 :     utm_in_fcu = res2.value();
+    1487             :   } else {
+    1488           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());
+    1489           0 :     return {};
+    1490             :   }
+    1491             : 
+    1492             :   // invert tf
+    1493        2074 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+    1494        2074 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+    1495             : 
+    1496        2074 :   return fcu_in_utm;
+    1497             : }
+    1498             : /*//}*/
+    1499             : 
+    1500             : /*//{ getAvgRtkInitZ() */
+    1501             : template <int n_measurements>
+    1502          22 : void Correction<n_measurements>::getAvgRtkInitZ(const double rtk_z) {
+    1503             : 
+    1504          22 :   if (!got_avg_init_rtk_z_) {
+    1505             : 
+    1506          22 :     double rtk_avg = rtk_init_z_avg_ / got_rtk_counter_;
+    1507             : 
+    1508          22 :     if (got_rtk_counter_ < 10 || (got_rtk_counter_ < 300 && std::fabs(rtk_z - rtk_avg) > 0.1)) {
+    1509             : 
+    1510          20 :       rtk_init_z_avg_ += rtk_z;
+    1511          20 :       got_rtk_counter_++;
+    1512          20 :       rtk_avg = rtk_init_z_avg_ / got_rtk_counter_;
+    1513          20 :       ROS_INFO("[%s]: RTK ASL altitude sample #%d: %.2f; avg: %.2f", getPrintName().c_str(), got_rtk_counter_, rtk_z, rtk_avg);
+    1514          20 :       return;
+    1515             : 
+    1516             :     } else {
+    1517             : 
+    1518           2 :       rtk_init_z_avg_     = rtk_avg;
+    1519           2 :       got_avg_init_rtk_z_ = true;
+    1520           2 :       ROS_INFO("[%s]: RTK ASL altitude avg: %f", getPrintName().c_str(), rtk_avg);
+    1521             :     }
+    1522             :   }
+    1523             : }
+    1524             : /*//}*/
+    1525             : 
+    1526             : /*//{ checkMsgDelay() */
+    1527             : template <int n_measurements>
+    1528             : void Correction<n_measurements>::checkMsgDelay(const ros::Time& msg_time) {
+    1529             : 
+    1530             :   const double delay = (ros::Time::now() - msg_time).toSec();
+    1531             :   if (delay > msg_delay_warn_limit_) {
+    1532             :     if (delay > msg_delay_limit_) {
+    1533             :       ROS_ERROR_THROTTLE(1.0, "[%s]: message too delayed (%.4f s)", getPrintName().c_str(), delay);
+    1534             :       is_delay_ok_ = false;
+    1535             :     } else {
+    1536             :       ROS_WARN_THROTTLE(5.0, "[%s]: message delayed (%.4f s)", getPrintName().c_str(), delay);
+    1537             :       is_delay_ok_ = true;
+    1538             :     }
+    1539             :   } else {
+    1540             :     is_delay_ok_ = true;
+    1541             :   }
+    1542             :   publishDelay(delay);
+    1543             : }
+    1544             : /*//}*/
+    1545             : 
+    1546             : /*//{ isTimestampOk() */
+    1547             : template <int n_measurements>
+    1548             : bool Correction<n_measurements>::isTimestampOk() {
+    1549             : 
+    1550             :   if (first_timestamp_) {
+    1551             :     return true;
+    1552             :   }
+    1553             : 
+    1554             :   ros::Time msg_time, prev_msg_time;
+    1555             :   {
+    1556             :     std::scoped_lock lock(mtx_msg_time_);
+    1557             :     msg_time      = msg_time_;
+    1558             :     prev_msg_time = prev_msg_time_;
+    1559             :   }
+    1560             :   const double delta = msg_time.toSec() - prev_msg_time.toSec();
+    1561             : 
+    1562             :   if (msg_time.toSec() <= 0.0) {
+    1563             :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    1564             :     return false;
+    1565             :   }
+    1566             : 
+    1567             :   if (delta <= 0.0) {
+    1568             :     ROS_WARN_THROTTLE(1.0, "[%s]: time delta non-positive: %f", getPrintName().c_str(), delta);
+    1569             :     return true;
+    1570             :   }
+    1571             : 
+    1572             :   if (delta < 0.001) {
+    1573             :     ROS_WARN_THROTTLE(1.0, "[%s]: time delta too small: %f", getPrintName().c_str(), delta);
+    1574             :     return true;
+    1575             :   }
+    1576             : 
+    1577             :   if (delta > time_since_last_msg_limit_) {
+    1578             :     ROS_ERROR_THROTTLE(1.0, "[%s]: time since last msg too long %f > %f", getPrintName().c_str(), delta, time_since_last_msg_limit_);
+    1579             :     return false;
+    1580             :   }
+    1581             : 
+    1582             :   return true;
+    1583             : }  // namespace mrs_uav_state_estimators
+    1584             : /*//}*/
+    1585             : 
+    1586             : /*//{ isMsgComing() */
+    1587             : template <int n_measurements>
+    1588      188230 : bool Correction<n_measurements>::isMsgComing() {
+    1589             : 
+    1590      188230 :   const ros::Time msg_time = mrs_lib::get_mutexed(mtx_msg_time_, msg_time_);
+    1591      188153 :   const double    delta    = ros::Time::now().toSec() - msg_time.toSec();
+    1592             : 
+    1593      188151 :   if (msg_time.toSec() <= 0.0) {
+    1594           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    1595           0 :     return false;
+    1596             :   }
+    1597             : 
+    1598      188118 :   if (delta > time_since_last_msg_limit_) {
+    1599           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: time since last msg too long %f > %f", getPrintName().c_str(), delta, time_since_last_msg_limit_);
+    1600           0 :     return false;
+    1601             :   }
+    1602             : 
+    1603      188118 :   return true;
+    1604             : }  // namespace mrs_uav_state_estimators
+    1605             : /*//}*/
+    1606             : 
+    1607             : /*//{ createProcessorFromName() */
+    1608             : template <int n_measurements>
+    1609          71 : std::shared_ptr<Processor<n_measurements>> Correction<n_measurements>::createProcessorFromName(const std::string& name, ros::NodeHandle& nh) {
+    1610             : 
+    1611          71 :   if (name == "median_filter") {
+    1612          10 :     return std::make_shared<ProcMedianFilter<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1613          61 :   } else if (name == "saturate") {
+    1614          14 :     return std::make_shared<ProcSaturate<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_, state_id_, fun_get_state_);
+    1615          47 :   } else if (name == "excessive_tilt") {
+    1616          10 :     return std::make_shared<ProcExcessiveTilt<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1617          37 :   } else if (name == "tf_to_world") {
+    1618          37 :     return std::make_shared<ProcTfToWorld<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    1619             :   } else {
+    1620           0 :     ROS_ERROR("[%s]: requested invalid processor %s", getPrintName().c_str(), name.c_str());
+    1621           0 :     ros::shutdown();
+    1622             :   }
+    1623           0 :   return std::shared_ptr<Processor<n_measurements>>(nullptr);
+    1624             : }
+    1625             : /*//}*/
+    1626             : 
+    1627             : /*//{ process() */
+    1628             : template <int n_measurements>
+    1629      386333 : bool Correction<n_measurements>::process(Correction<n_measurements>::measurement_t& measurement) {
+    1630             : 
+    1631      386333 :   bool ok_flag   = true;
+    1632      386333 :   bool fuse_flag = true;
+    1633             : 
+    1634      525245 :   for (auto proc_name :
+    1635             :        processor_names_) {  // need to access the processors in the specific order from the config (e.g. median filter should go before saturation etc.)
+    1636             :     /* bool is_ok, should_fuse; */
+    1637      138912 :     auto [is_ok, should_fuse] = processors_[proc_name]->process(measurement);
+    1638      138912 :     ok_flag &= is_ok;
+    1639      138912 :     fuse_flag &= should_fuse;
+    1640             :   }
+    1641      386325 :   if (fuse_flag) {
+    1642      385334 :     if (!ok_flag) {
+    1643           1 :       setR(default_R_ * R_coeff_);
+    1644           1 :       ROS_INFO_THROTTLE(1.0, "[%s]: set R to %.4f", getPrintName().c_str(), default_R_ * R_coeff_);
+    1645           1 :       return true;
+    1646             :     } else {
+    1647      385333 :       setR(default_R_);
+    1648      385301 :       return true;
+    1649             :     }
+    1650             :   }
+    1651         991 :   return false;
+    1652             : }
+    1653             : /*//}*/
+    1654             : 
+    1655             : /*//{ resetProcessors() */
+    1656             : template <int n_measurements>
+    1657           0 : void Correction<n_measurements>::resetProcessors() {
+    1658             : 
+    1659           0 :   for (auto proc_name :
+    1660             :        processor_names_) {  // need to access the processors in the specific order from the config (e.g. median filter should go before saturation etc.)
+    1661             :     /* bool is_ok, should_fuse; */
+    1662           0 :     processors_[proc_name]->reset();
+    1663             :   }
+    1664           0 : }
+    1665             : /*//}*/
+    1666             : 
+    1667             : /*//{ publishCorrection() */
+    1668             : template <int n_measurements>
+    1669      771627 : void Correction<n_measurements>::publishCorrection(const MeasurementStamped&                                 measurement_stamped,
+    1670             :                                                    mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr) {
+    1671             : 
+    1672      771627 :   if (!ch_->debug_topics.correction) {
+    1673           0 :     return;
+    1674             :   }
+    1675             : 
+    1676     1543369 :   mrs_msgs::EstimatorCorrection msg;
+    1677      771485 :   msg.header.stamp    = measurement_stamped.stamp;
+    1678      771485 :   msg.header.frame_id = ns_frame_id_;
+    1679      771716 :   msg.name            = name_;
+    1680      771720 :   msg.estimator_name  = est_name_;
+    1681      771745 :   msg.state_id        = state_id_;
+    1682      771745 :   msg.covariance.resize(n_measurements * n_measurements);
+    1683     1748461 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+    1684      977079 :     msg.state.push_back(measurement_stamped.value(i));
+    1685      977186 :     msg.covariance[n_measurements * i + i] = getR();
+    1686             :   }
+    1687             : 
+    1688      771199 :   ph_corr.publish(msg);
+    1689             : }
+    1690             : /*//}*/
+    1691             : 
+    1692             : /*//{ publishDelay() */
+    1693             : template <int n_measurements>
+    1694             : void Correction<n_measurements>::publishDelay(const double delay) {
+    1695             : 
+    1696             :   if (!ch_->debug_topics.corr_delay) {
+    1697             :     return;
+    1698             :   }
+    1699             : 
+    1700             :   mrs_msgs::Float64Stamped msg;
+    1701             :   msg.header.stamp    = ros::Time::now();
+    1702             :   msg.header.frame_id = ns_frame_id_;
+    1703             :   msg.value           = delay;
+    1704             : 
+    1705             :   ph_delay_.publish(msg);
+    1706             : }
+    1707             : /*//}*/
+    1708             : 
+    1709             : }  // namespace mrs_uav_state_estimators
+    1710             : 
+    1711             : #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..327e371026 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html @@ -0,0 +1,448 @@ + + + + + + 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 + + +
+ 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..dbce9ae56aa0542137b4a0f1b6433f5303feccec GIT binary patch literal 5088 zcmV<66Cdn}P)A#hY^@7AF#~vqH>-KhMG7JI)2!WpNa=8qbOQ$yv znA-3@pt&t*dXMdWWF^Mfwg;p+F9BKk5E$v+Yd{M|uG=Pz=7upgb66W0c(KmEQ%OgXiR-8fDITgM!-$EZpO8u>%r3DR_cnnwoP&N zuFVoNkCPQ{unyS5bOIiR4F>p2RH5=HI#uh*A<(mdk0`x$=9k(!-)(!CT@ z9BjS4KiqXP5%IU%T<&1;L780z&fK-JHH{1$m?6o(4}bMgEqc=53{Y1SU=0_+hDJrp zzodu~X_o|z0Q)Th{EN3R9g`y|^(!XMN@2xHQR-XkE3qX)ktYERGoU^R_6oDjN(eT&Qhz-_GX0<^5JV~U18R(Cx> zK(0rlco5*5G!j;i0ho__p2kwV=ix2Q_ z0~`dvA=pf_DpxZUhlT=VJRyaVkR4bwYF!u1_DWa?g!iUxhYo25D!?KBBLf1&?@gd)&VImYI+sdus%akO(Dl1ZZ8_`{d028W;p9u@`@; z(r9kBVXlme$wf9r?FBl_3={>?QJ>_$(h6xq24RB72y*}z!Siij;>;;Q70{T9zwvE) z{kQyC+Spy|HoiVju>P;8qnAPGhn9f8D=Ui zWNA!7pD5)nm<4n;#IkXpi8-@2dX}7;R3xppm06(ZCLub-0rPPTl_>LU!-FcBPT0jHfXY z3aImh*}L9jM}FUfJ#m+~*Vz5jqX4N|eWdTe68{EPNL0&%3jtrM7PSq0nZ*;nn~~_M zw!@mMQI15|oOX+0IjX(_E&;f?D0&KjO9Zs870fjHkr9Vw-fLtQf%h@B9c7+%nHRWk z)ihG(02X&LL#2HLZjQ|XpmAmgv^{w>@yi4cXjGnSC((rf?L!?0RR?TI?o;`rLf7Lm zMPWgg7V*>> zD=`Tez*(;Am{|Z+Q-wb$aAJyE^mD{wZxQ%7T14~#vw|+lCc7rhdc^TUwrho@AT;%N+Zvbccb zMkYdn1p@`U0NxXrz}Nz4;=6O(BrHcNW6y~~KG%HMialf>W2zD@z;Wnm*GHmQO#yy@ zH&gw6TM&n4L(1*9ZIfv8X1ez5wtz%TGeu-G*TzDQ8DmSu$0nY8_B{f=?0eL)Y4JVA zcK2+j-^@;)UZ)tFb^)cpnntE1R$y#tN4aKm3vw*>T~OQcym-`Y!%-&?>Bn<+$Z!NNQeMO4P%d_-7^Bl5fWdiXd?V327@-=oH63*c!r-8h$_oEyD zz4!WP2apineCZ+l7e5L)sUsElBV5YX;mjEUd}9P~{ep@8Zd@$b7iZUxu|jdLtF7xR z)<+rR$5~-{cJH)8Ml_J?GrXn&&28@->3>xusq>>thGHd+-KQlg>5(}Tl{}!itvL(u zjH$3Wr@`V@<9V0-MC4%bS}sA~g8-9LNHumm8GM9I4FSPMq80#LE8&d7A&TJnpq z?O}!bOpJU_6neNAV1_;$_#OjB%aq;3^f6L&J;xnd@)fYv6vz0z|9px&XD&*d^2|j6 z$}`vV@vAe}FHK12Jman6oM&9m=TeHZtq3S)M?7F9OYW72JLnq-vs!BdN9q?4^D{|{ zps}pm5hN3FINkRiQDParGoGrxcg8IDeedkGq`O-bNuITu zS-Ae*B1uimStN;$nSZ@(>mo@}oeRHHs*;_Vvznq|{G(LU`+%RHlR78%_uHAr-O{*D zn_)5jpv{~U5({|dq$Kg2IjR35Clz@A8oguE54Izx zE4Ne^(klVYTY;_2?4Ex3gYEP2G{`D76UPT9hS+;-;=2GxUF%rrqWp9s5Zajj9sOMc z>4t%u=_>8r9}(nlCS7w>!hKJiRHy(Q0QER01E@Z+)&^Leq#fhyY)xF(CvxG~hqm&i zQ|3HwD~+xGQ8{AVf%74vX4)SI;JaIXKc-;L zA)PUWROCK1rYObDKJH}3NW7>`0hVjMn5jS+iw}!ZICK13W`1O3#`0|SNHg<@T+Eom z!Nsc|9Py3nzDOv$j;2U z`p_^oM^eHvoC&$M0X)MQ7UR!^GynZEWn<$yZRYoF#wJB>2~bJ#*|Y>W?p{}OOMy&0 z66)_K?8ECI!^MB#0DH?2enxC2pC{d534jB!-(LbyKJZ)u$ji;YS_0H-06R115=d|M#C{F?-_DQ?OdiP`e?-X zmIH{UI~9PZ17HBX0-X-vCmg_!9G0}4mW-#^)I*TzG$1}tSW`do2YwJyj)cH(o3Jo_QY9!|Kh1y9(LXN;-)7KA7qoHf--HOK;Vr2RBX79n7 z+mDu0m6ED?k&E3}-INPzygs4}V9jlh3u>%$TzNsA`sA3Q`L}!xc=*fT`b<&=G+zE@ zy3}ps*MYnEXj`8|&f+fx{yr%eqr87r;JuRQ+DF5OjgSS5T_1;0#JK!S9YX-9kKxj_ zX>6=FD>|}tu=`JyG>Y-~l}d&3{^aHDN=5v^J#m z1AY-WT%%^DJFhGlqr}tcOlTT21TUOBze z^HWRJ-poxOO?bFSd5>#J5!W@ST*)-<1$dpb$rFVNARMhOi|MW<6pPW=_*-I%jUes2 zam4ME5Ri|+A>e*@x{QI=$(`-;$GgU);kO^Ws%67NEKrKP67AtBtj(y7Yfr`t&L;py z^;Q=Z4p8fQ%r-L##?{5>TFvj70UHHg>Hx7WEIv#73s4(Z_#Szu0tCF`yB^*I{0k=) zu53%LYWh_trRnVoCzWJLwYM(%nP?5SVl3)CgTEz%V9L$cq@4ieyv6xi+ytCxM88|1 z&VcV+jq!lj@;>~bSGfA9m+qiQcjO%!VPtf;E0R~fU9!|R@`6L=~7?f!iWcQO=2tnxy{@dkmotZxXBVYDK_u3Z!j(;T;IgFV(I1KKlwJB z3FAn4Hf##vBiaSF`EXtf0iZcYy6ar{Cr3`QQm?$CPB4j?BwQl zvx>1t9*Q?n$v3Pe@sDoEukr^?tKnD!BT`m*r5pj>QE{9~!7G+k3&>GQg3vea#&b3? z{{gavN9@K4i6JCA(;erY`1u(UIPfwh3v9v!PPSg=LR%fPA9|)uv!Ga_TYCc)MjAl+v@-+VEBJIk4+dmm*Ht#q7QY7#zd2 z{Kkl9o++h1^|`Av-9u1xtty4NYrWrV(@}{a6cyuVlr%Qlt;qV@Vk30Re%!bvRo{>M z$i2m*PjlSPn%;uw%nqR5q|J40osi7)%u|msJ>Z(L-fSy}Ax^kS&c674J2UlzKeXF8 z_-ZvYPbeGHZgm}%1x%9>7sTb?7B|Mi#~2l7pD8i{qM?anh0o8pI?y;y^~8#10qUR) zDWD2)T2K?)`1XA!Q=X7M!r3baPt6X1tx5&w+E|ftC`*idvB@Z){Z6L_Iq85gBhHeX z1vf}wA-V6qk~ier8yqzW?!eNJIa_FM3j(BR4BS6+TwY5awPT(D0000 + + + + + + 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:2023-12-06 07:59:45Functions: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..06106cc2cc --- /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:2023-12-06 07:59:45Functions: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..4a90f2f798 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html @@ -0,0 +1,240 @@ + + + + + + + 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:2023-12-06 07:59:45Functions: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 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<lkf_t> rep_lkf_t;
+      55             : 
+      56             :   using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;
+      57             : 
+      58             : private:
+      59             :   std::string parent_state_est_name_;
+      60             : 
+      61             :   double                              dt_;
+      62             :   double                              input_coeff_;
+      63             :   double                              default_input_coeff_;
+      64             :   A_t                                 A_;
+      65             :   B_t                                 B_;
+      66             :   H_t                                 H_;
+      67             :   Q_t                                 Q_;
+      68             :   std::shared_ptr<lkf_t>              lkf_;
+      69             :   std::unique_ptr<rep_lkf_t>          lkf_rep_;
+      70             :   std::vector<std::shared_ptr<lkf_t>> models_;
+      71             :   mutable std::mutex                  mutex_lkf_;
+      72             :   statecov_t                          sc_;
+      73             :   mutable std::mutex                  mutex_sc_;
+      74             : 
+      75             :   std::unique_ptr<drmgr_t> drmgr_;
+      76             :   void                     callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level);
+      77             : 
+      78             :   z_t                innovation_;
+      79             :   mutable std::mutex mtx_innovation_;
+      80             : 
+      81             :   bool is_repredictor_enabled_;
+      82             :   int  rep_buffer_size_ = 200;
+      83             : 
+      84             :   const bool is_core_plugin_;
+      85             : 
+      86             :   std::vector<std::string>                                              correction_names_;
+      87             :   std::vector<std::shared_ptr<Correction<hdg_generic::n_measurements>>> corrections_;
+      88             : 
+      89             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput> sh_control_input_;
+      90             :   void                                                timeoutCallback(const std::string &topic, const ros::Time &last_msg);
+      91             :   std::atomic<bool>                                   is_input_ready_ = false;
+      92             : 
+      93             :   ros::Timer timer_update_;
+      94             :   void       timerUpdate(const ros::TimerEvent &event);
+      95             : 
+      96             :   ros::Timer timer_check_health_;
+      97             :   void       timerCheckHealth(const ros::TimerEvent &event);
+      98             : 
+      99             :   void doCorrection(const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id);
+     100             :   void doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp);
+     101             : 
+     102             :   bool isConverged();
+     103             : 
+     104             :   Q_t                getQ();
+     105             :   mutable std::mutex mtx_Q_;
+     106             : 
+     107             :   mutable std::mutex mutex_last_valid_hdg_;
+     108             :   double             last_valid_hdg_;
+     109             : 
+     110             : public:
+     111           0 :   HdgGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     112           0 :       : HeadingEstimator<hdg_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     113           0 :   }
+     114             : 
+     115           0 :   ~HdgGeneric(void) {
+     116           0 :   }
+     117             : 
+     118             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     119             :   bool start(void) override;
+     120             :   bool pause(void) override;
+     121             :   bool reset(void) override;
+     122             : 
+     123             :   double getState(const int &state_idx_in) const override;
+     124             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     125             : 
+     126             :   void setState(const double &state_in, const int &state_idx_in) override;
+     127             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     128             : 
+     129             :   states_t getStates(void) const override;
+     130             :   void     setStates(const states_t &states_in) override;
+     131             : 
+     132             :   double getCovariance(const int &state_idx_in) const override;
+     133             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     134             : 
+     135             :   covariance_t getCovarianceMatrix(void) const override;
+     136             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     137             : 
+     138             :   double getInnovation(const int &state_idx) const override;
+     139             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     140             : 
+     141             :   double getLastValidHdg() const override;
+     142             : 
+     143             :   void setDt(const double &dt);
+     144             :   void setInputCoeff(const double &input_coeff);
+     145             : 
+     146             :   void generateA();
+     147             :   void generateB();
+     148             : 
+     149             : 
+     150             :   std::string getNamespacedName() const;
+     151             : 
+     152             :   std::string getPrintName() const;
+     153             : };
+     154             : }  // namespace mrs_uav_state_estimators
+     155             : 
+     156             : #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..90d85a2a54 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html @@ -0,0 +1,59 @@ + + + + + + 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 + + +
+ 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..184fcd89b0f359d12d16f08ceb9b076b8700fff8 GIT binary patch literal 655 zcmV;A0&x9_P)0{{R38Wk~70000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpSTPn!rauAj>G z&`{Epo6QEOh>?TlaS6n$y7Q}{^By{A + + + + + + 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:2023-12-06 07:59:45Functions: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)39
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()39
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().239
+
+
+ + + +
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..411a95cb96 --- /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:2023-12-06 07:59:45Functions: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)39
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()39
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().239
+
+
+ + + +
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..2d7d7aeafa --- /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:2023-12-06 07:59:45Functions: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          39 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70          39 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71          39 :   }
+      72             : 
+      73          78 :   ~HdgPassthrough(void) {
+      74          78 :   }
+      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..bfe03e614c --- /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:2023-12-06 07:59:45Functions: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&)39
+
+
+ + + +
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..8583dfc0d4 --- /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:2023-12-06 07:59:45Functions: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&)39
+
+
+ + + +
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..827549e560 --- /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:2023-12-06 07:59:45Functions: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          39 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23          39 :   }
+      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..a5e6c19e5b --- /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:2023-12-06 07:59:45Functions: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..bd7b357d9f --- /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:2023-12-06 07:59:45Functions: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..67fce59819 --- /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:2023-12-06 07:59:45Functions: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..299c1d7732 --- /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:2023-12-06 07:59:45Functions: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..df09e2deb6 --- /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:2023-12-06 07:59:45Functions: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..79197fa788 --- /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:2023-12-06 07:59:45Functions: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..78894e2b75 --- /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:40469158.5 %
Date:2023-12-06 07:59:45Functions:689174.7 %
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 +
56.5%56.5%
+ 		56.5 %370 / 65573.1 %49 / 67
<unnamed>56.5 %370 / 65573.1 %49 / 67
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..e594060492 --- /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:40469158.5 %
Date:2023-12-06 07:59:45Functions:689174.7 %
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 +
56.5%56.5%
+ 		56.5 %370 / 65573.1 %49 / 67
<unnamed>56.5 %370 / 65573.1 %49 / 67
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..607d810297 --- /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:40469158.5 %
Date:2023-12-06 07:59:45Functions:689174.7 %
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 +
56.5%56.5%
+ 		56.5 %370 / 65573.1 %49 / 67
<unnamed>56.5 %370 / 65573.1 %49 / 67
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..0fd485c901 --- /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:40469158.5 %
Date:2023-12-06 07:59:45Functions:689174.7 %
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 +
56.5%56.5%
+ 		56.5 %370 / 65573.1 %49 / 67
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..5dcba363c1 --- /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:40469158.5 %
Date:2023-12-06 07:59:45Functions:689174.7 %
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 +
56.5%56.5%
+ 		56.5 %370 / 65573.1 %49 / 67
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..50b045e5b9 --- /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:40469158.5 %
Date:2023-12-06 07:59:45Functions:689174.7 %
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 +
56.5%56.5%
+ 		56.5 %370 / 65573.1 %49 / 67
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..64e0ac0638 --- /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:99100.0 %
Date:2023-12-06 07:59:45Functions: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 %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/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..5ac7e530d1 --- /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:99100.0 %
Date:2023-12-06 07:59:45Functions: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 %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/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..33ff2bb55f --- /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:99100.0 %
Date:2023-12-06 07:59:45Functions: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 %5 / 5100.0 %3 / 3
<unnamed>100.0 %5 / 5100.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..e3885e4a34 --- /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:99100.0 %
Date:2023-12-06 07:59:45Functions: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 %5 / 5100.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..e6ad21a192 --- /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:99100.0 %
Date:2023-12-06 07:59:45Functions: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 %5 / 5100.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..cf41472683 --- /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:99100.0 %
Date:2023-12-06 07:59:45Functions: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 %5 / 5100.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..663696a4c8 --- /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:55100.0 %
Date:2023-12-06 07:59:45Functions: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> ()>)39
mrs_uav_state_estimators::LatGeneric::~LatGeneric()39
mrs_uav_state_estimators::LatGeneric::~LatGeneric().239
+
+
+ + + +
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..c4088abc33 --- /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:55100.0 %
Date:2023-12-06 07:59:45Functions: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> ()>)39
mrs_uav_state_estimators::LatGeneric::~LatGeneric()39
mrs_uav_state_estimators::LatGeneric::~LatGeneric().239
+
+
+ + + +
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..07e68284bb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html @@ -0,0 +1,244 @@ + + + + + + + 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:55100.0 %
Date:2023-12-06 07:59:45Functions: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 A_t        = lkf_t::A_t;
+      43             :   using B_t        = lkf_t::B_t;
+      44             :   using H_t        = lkf_t::H_t;
+      45             :   using Q_t        = lkf_t::Q_t;
+      46             :   using x_t        = lkf_t::x_t;
+      47             :   using P_t        = lkf_t::P_t;
+      48             :   using u_t        = lkf_t::u_t;
+      49             :   using z_t        = lkf_t::z_t;
+      50             :   using R_t        = lkf_t::R_t;
+      51             :   using statecov_t = lkf_t::statecov_t;
+      52             : 
+      53             :   typedef mrs_lib::Repredictor<lkf_t> rep_lkf_t;
+      54             : 
+      55             : private:
+      56             :   const std::string package_name_ = "mrs_uav_state_estimators";
+      57             : 
+      58             :   std::string parent_state_est_name_;
+      59             : 
+      60             :   double                              dt_;
+      61             :   double                              input_coeff_, default_input_coeff_;
+      62             :   A_t                                 A_;
+      63             :   B_t                                 B_;
+      64             :   H_t                                 H_;
+      65             :   Q_t                                 Q_;
+      66             :   std::shared_ptr<lkf_t>              lkf_;
+      67             :   std::unique_ptr<rep_lkf_t>          lkf_rep_;
+      68             :   std::vector<std::shared_ptr<lkf_t>> models_;
+      69             :   mutable std::mutex                  mutex_lkf_;
+      70             :   statecov_t                          sc_;
+      71             :   mutable std::mutex                  mutex_sc_;
+      72             : 
+      73             :   std::unique_ptr<drmgr_t> drmgr_;
+      74             :   void                     callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level);
+      75             : 
+      76             :   z_t                innovation_;
+      77             :   mutable std::mutex mtx_innovation_;
+      78             : 
+      79             :   bool is_error_state_first_time_ = true;
+      80             :   ros::Duration error_state_duration_;
+      81             :   ros::Time prev_time_in_error_state_;
+      82             : 
+      83             :   bool is_repredictor_enabled_;
+      84             :   int  rep_buffer_size_ = 200;
+      85             : 
+      86             :   const bool is_core_plugin_;
+      87             : 
+      88             :   std::vector<std::string>                                              correction_names_;
+      89             :   std::vector<std::shared_ptr<Correction<lat_generic::n_measurements>>> corrections_;
+      90             : 
+      91             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput> sh_control_input_;
+      92             :   void                                                timeoutCallback(const std::string &topic, const ros::Time &last_msg);
+      93             :   std::atomic<bool>                                   is_input_ready_ = false;
+      94             : 
+      95             : 
+      96             :   std::function<std::optional<double>()>               fun_get_hdg_;
+      97             :   std::string                                          hdg_source_topic_;
+      98             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput> sh_hdg_state_;
+      99             :   std::atomic<bool>                                    is_hdg_state_ready_ = false;
+     100             : 
+     101             :   ros::Timer timer_update_;
+     102             :   void       timerUpdate(const ros::TimerEvent &event);
+     103             : 
+     104             :   ros::Timer timer_check_health_;
+     105             :   void       timerCheckHealth(const ros::TimerEvent &event);
+     106             : 
+     107             : 
+     108             :   void doCorrection(const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id);
+     109             :   void doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp);
+     110             : 
+     111             :   bool isConverged();
+     112             : 
+     113             :   Q_t                getQ();
+     114             :   mutable std::mutex mtx_Q_;
+     115             : 
+     116             : public:
+     117          39 :   LatGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin,
+     118             :              std::function<std::optional<double>()> fun_get_hdg)
+     119          39 :       : LateralEstimator<lat_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin), fun_get_hdg_(fun_get_hdg) {
+     120          39 :   }
+     121             : 
+     122          78 :   ~LatGeneric(void) {
+     123          78 :   }
+     124             : 
+     125             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     126             :   bool start(void) override;
+     127             :   bool pause(void) override;
+     128             :   bool reset(void) override;
+     129             : 
+     130             :   double getState(const int &state_idx_in) const override;
+     131             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     132             : 
+     133             :   void setState(const double &state_in, const int &state_idx_in) override;
+     134             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     135             : 
+     136             :   states_t getStates(void) const override;
+     137             :   void     setStates(const states_t &states_in) override;
+     138             : 
+     139             :   double getCovariance(const int &state_idx_in) const override;
+     140             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     141             : 
+     142             :   covariance_t getCovarianceMatrix(void) const override;
+     143             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     144             : 
+     145             :   double getInnovation(const int &state_idx) const override;
+     146             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     147             : 
+     148             :   void setDt(const double &dt);
+     149             :   void setInputCoeff(const double &input_coeff);
+     150             : 
+     151             :   void generateA();
+     152             :   void generateB();
+     153             : 
+     154             :   std::string getNamespacedName() const;
+     155             : 
+     156             :   std::string getPrintName() const;
+     157             : };
+     158             : }  // namespace mrs_uav_state_estimators
+     159             : 
+     160             : #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..56d2bbd4e6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_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/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 + + +
+ 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..f922dc5e2df8de7ab7fe1ec72ccf6d7aab4af7d1 GIT binary patch literal 682 zcmV;b0#*HqP)#j#a4B1xM%6g0$cLW?W z{sYedbNSFLJhG;GaN>f2(D*bhtX%rik4O#+x40pXlovd;wa0_Wz8Os!R2G+BXPeS35%Z|eDxN8qorRp*|ALE; zf=R!COm2Vbs@H8l(IBJjM_ual^DbWv270Ul$W?%ycZ*XHjeCz1{SNtgQ{3z$)T)|Q z!1&z|@Q}{&&~0!N>0{qYqS=q7S+hT)Y>nc2C;Q3ASNwbDW4d-%w4Zll33eq5z*mcs zgu&ePx=iM5umB{bG4&Uuo40GcZuSgNfG}H3=YeJ`eS&?FYXS-oX-sQzO_~VP5GyZe zqdEf8cr9w)XZz_4w?#uHKX!YE=d}VXvYJ!}0*zt0hp@aBUM=|hx(4RJcZyF9Mic)= z)TB0kO-N;`!^KE7#%ZaTfeX^Ugy2`#$CMHJzM6H!zwDkT`*zVaomlbn7roo7qWsP3 Qf&c&j07*qoM6N<$f<09`?EnA( literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..c22f7ba7c0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.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/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:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().239
+
+
+ + + +
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..26e784b047 --- /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:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().239
+
+
+ + + +
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..af710df382 --- /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:2023-12-06 07:59:45Functions: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          39 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24          39 :   }
+      25             : 
+      26          39 :   ~LateralEstimator(void) {
+      27          39 :   }
+      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..3350b9e286 --- /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:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().239
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&)39
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().239
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&)74
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().274
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const14205
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&) const48466
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const48466
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const48466
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const48466
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const48653
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const48653
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const48653
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&) const89243
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const89396
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const89402
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const89419
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const775440
+
+
+ + + +
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..d9c6b9318d --- /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:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().239
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&)74
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().274
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&)39
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().239
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() const48653
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const48653
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const48653
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&) const89243
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const89402
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const14205
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const89419
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const89396
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&) const48466
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const48466
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const775440
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const48466
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const48466
+
+
+ + + +
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..47edeff46b --- /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:2023-12-06 07:59:45Functions: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         152 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62         152 :   }
+      63             : 
+      64         152 :   ~PartialEstimator(void) {
+      65         152 :   }
+      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      186538 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103      186538 :   const states_t      states = getStates();
+     104      186497 :   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      842763 :   for (int i = 0; i < states.size(); i++) {
+     109      656186 :     states_vec.push_back(states(i));
+     110             :   }
+     111      372992 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117      186515 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118      186515 :   const covariance_t  covariance = getCovarianceMatrix();
+     119      186485 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123      842455 :   for (int i = 0; i < covariance.rows(); i++) {
+     124     3399392 :     for (int j = 0; j < covariance.cols(); j++) {
+     125     2743122 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128      372974 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134      789645 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135      789645 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141      186521 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143      186521 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147      373022 :   mrs_msgs::EstimatorOutput msg;
+     148      186473 :   msg.header.stamp    = ros::Time::now();
+     149      186541 :   msg.header.frame_id = getFrameId();
+     150      186534 :   msg.state           = getStatesAsVector();
+     151      186440 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153      186446 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160      137709 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162      137709 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166      274919 :   mrs_msgs::Float64ArrayStamped msg;
+     167      137149 :   msg.header.stamp = stamp;
+     168      323216 :   for (int i = 0; i < u.rows(); i++) {
+     169      185370 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172      137422 :   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:2023-12-06 07:59:45Functions: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..e8cc050d32 --- /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:2023-12-06 07:59:45Functions: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..189f03cee9 --- /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:2023-12-06 07:59:45Functions: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..727fa1a781 --- /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:2023-12-06 07:59:45Functions: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..791bb7dcb0 --- /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:2023-12-06 07:59:45Functions: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..d74aefe51f --- /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:2023-12-06 07:59:45Functions: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..69828c912b --- /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:2023-12-06 07:59:45Functions: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()3
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough()3
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough().23
+
+
+ + + +
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..066aee7344 --- /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:2023-12-06 07:59:45Functions: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()3
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough()3
mrs_uav_state_estimators::passthrough::Passthrough::~Passthrough().23
+
+
+ + + +
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..7edfd6ce37 --- /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:2023-12-06 07:59:45Functions: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           3 :   Passthrough() : StateEstimator(passthrough::name, passthrough::frame_id, passthrough::package_name), is_core_plugin_(is_core_plugin) {
+      67           3 :   }
+      68             : 
+      69           6 :   ~Passthrough(void) {
+      70           6 :   }
+      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:2023-12-06 07:59:45Functions: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)39
mrs_uav_state_estimators::StateGeneric::~StateGeneric().239
+
+
+ + + +
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..40a52560c8 --- /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:2023-12-06 07:59:45Functions: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)39
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::~StateGeneric().239
+
+
+ + + +
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..1283470e65 --- /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:2023-12-06 07:59:45Functions: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          39 :   StateGeneric(const std::string &name, const bool is_core_plugin)
+      79          39 :       : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) {
+      80          39 :   }
+      81             : 
+      82          39 :   ~StateGeneric(void) {
+      83          39 :   }
+      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..d933c1c4b2 --- /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:12517471.8 %
Date:2023-12-06 07:59:45Functions: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.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
<unnamed>78.7 %37 / 4737.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..5ac639d15e --- /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:12517471.8 %
Date:2023-12-06 07:59:45Functions: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.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
<unnamed>78.7 %37 / 4737.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..3797119f11 --- /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:12517471.8 %
Date:2023-12-06 07:59:45Functions: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.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
<unnamed>78.7 %37 / 4737.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..700f0eef32 --- /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:12517471.8 %
Date:2023-12-06 07:59:45Functions: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.7%78.7%
+ 		78.7 %37 / 4737.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..e0c7014a83 --- /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:12517471.8 %
Date:2023-12-06 07:59:45Functions: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.7%78.7%
+ 		78.7 %37 / 4737.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..f18d243c87 --- /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:12517471.8 %
Date:2023-12-06 07:59:45Functions: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.7%78.7%
+ 		78.7 %37 / 4737.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..cbe858c3cd --- /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:2023-12-06 07:59:45Functions: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&)10
mrs_uav_state_estimators::ProcExcessiveTilt<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)13196
+
+
+ + + +
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..c12459d09e --- /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:2023-12-06 07:59:45Functions: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>&)13196
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&)10
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..3764dba88d --- /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:2023-12-06 07:59:45Functions: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          10 : 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          10 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      45             : 
+      46             :   // | --------------------- load parameters -------------------- |
+      47          10 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      48             : 
+      49          10 :   ph->param_loader->loadParam("orientation_topic", orientation_topic_);
+      50             :   double max_tilt;
+      51          10 :   ph->param_loader->loadParam("max_tilt", max_tilt);
+      52             : 
+      53          10 :   max_tilt = M_PI * (max_tilt / 180.0);
+      54             : 
+      55          10 :   max_tilt_sq_ = std::pow(max_tilt, 2);
+      56             : 
+      57          10 :   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          10 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          10 :   shopts.nh                 = nh;
+      65          10 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          10 :   shopts.no_message_timeout = ros::Duration(1.0);
+      67          10 :   shopts.threadsafe         = true;
+      68          10 :   shopts.autostart          = true;
+      69          10 :   shopts.queue_size         = 10;
+      70          10 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          10 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch->uav_name + "/" + orientation_topic_);
+      73          10 : }
+      74             : /*//}*/
+      75             : 
+      76             : /*//{ process() */
+      77             : template <int n_measurements>
+      78       13196 : std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process(measurement_t& measurement) {
+      79             : 
+      80       13196 :   if (!Processor<n_measurements>::enabled_) {
+      81           0 :     return {true, true};
+      82             :   }
+      83             : 
+      84       13196 :   if (!sh_orientation_.hasMsg()) {
+      85           0 :     return {false, false};
+      86             :   }
+      87             : 
+      88       13196 :   bool ok_flag     = true;
+      89       13196 :   bool should_fuse = true;
+      90             : 
+      91             :   try {
+      92       13196 :     Eigen::Matrix3d orientation_R = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion);
+      93             : 
+      94       13196 :     const double tilt = mrs_lib::geometry::angleBetween(Eigen::Vector3d(0, 0, 1), orientation_R.col(2));
+      95             : 
+      96       13196 :     const bool is_excessive_tilt = std::pow(tilt, 2) > max_tilt_sq_;
+      97             : 
+      98       13196 :     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       13195 :   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..23cf1cfb7b --- /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:2023-12-06 07:59:45Functions: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&)10
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)13193
+
+
+ + + +
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..966df82510 --- /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:2023-12-06 07:59:45Functions: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>&)13193
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&)10
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..f0ae73b719 --- /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:2023-12-06 07:59:45Functions: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          10 : 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          10 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43          10 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45          10 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46          10 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48          10 :   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          10 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55          10 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58          20 :   for (int i = 0; i < n_measurements; i++) {
+      59          10 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61          10 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66       13193 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68       13193 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72       13193 :   bool ok_flag     = true;
+      73       13193 :   bool should_fuse = true;
+      74       26389 :   for (int i = 0; i < measurement.rows(); i++) {
+      75       13196 :     vec_mf_[i].add(measurement(i));
+      76       13196 :     if (vec_mf_[i].full()) {
+      77       12206 :       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         990 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88         990 :       ok_flag     = false;
+      89         990 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93       13196 :   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..850a14dc4d --- /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:2023-12-06 07:59:45Functions: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)>)2
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)>)12
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)1043
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)14205
+
+
+ + + +
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..9a81aad729 --- /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:2023-12-06 07:59:45Functions: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>&)14205
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)>)12
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)1043
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)>)2
+
+
+ + + +
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..b930fb5ee3 --- /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:2023-12-06 07:59:45Functions: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          14 : 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          14 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48          14 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50          14 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51          14 :   this->enabled_ = this->start_enabled_;
+      52          14 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53          14 :   ph->param_loader->loadParam("min", saturate_min_);
+      54          14 :   ph->param_loader->loadParam("max", saturate_max_);
+      55          14 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57          14 :   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          14 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66       15248 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69       15248 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73       15248 :   bool ok_flag     = true;
+      74       15248 :   bool should_fuse = true;
+      75       31027 :   for (int i = 0; i < measurement.rows(); i++) {
+      76       16287 :     const double state = fun_get_state_(state_id_, i);
+      77       16287 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79       16287 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80         508 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83       15779 :     if (measurement(i) > state + saturate_max_) {
+      84         100 :       const double saturated = state + saturate_max_;
+      85         100 :       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         100 :       measurement(i) = saturated;
+      88         100 :       ok_flag        = false;
+      89         100 :       should_fuse    = true;
+      90       15679 :     } 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       14740 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105       14740 :   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..d0406f94bc --- /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:374778.7 %
Date:2023-12-06 07:59:45Functions: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&)37
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)23994
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)97272
+
+
+ + + +
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..93cf57f5a3 --- /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:374778.7 %
Date:2023-12-06 07:59:45Functions: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>)23994
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)97272
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&)37
+
+
+ + + +
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..6f601bd0e7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html @@ -0,0 +1,222 @@ + + + + + + + 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:374778.7 %
Date:2023-12-06 07:59:45Functions: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             :   bool   got_gnss_                  = false;
+      37             :   bool   is_gnss_offset_calculated_ = false;
+      38             :   double gnss_x_;
+      39             :   double gnss_y_;
+      40             : 
+      41             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+      42             :   void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+      43             : };
+      44             : 
+      45             : /*//{ constructor */
+      46             : template <int n_measurements>
+      47          37 : ProcTfToWorld<n_measurements>::ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      48             :                                              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+      49          37 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      50             : 
+      51          37 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      52             : 
+      53          37 :   ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      54             : 
+      55          37 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      56             : 
+      57          37 :   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          37 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          37 :   shopts.nh                 = nh;
+      65          37 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          37 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      67          37 :   shopts.threadsafe         = true;
+      68          37 :   shopts.autostart          = true;
+      69          37 :   shopts.queue_size         = 10;
+      70          37 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          37 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcTfToWorld::callbackGnss, this);
+      73             : 
+      74          37 :   is_initialized_ = true;
+      75          37 : }
+      76             : /*//}*/
+      77             : 
+      78             : /*//{ callbackGnss() */
+      79             : template <int n_measurements>
+      80       23994 : void ProcTfToWorld<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      81             : 
+      82       23994 :   if (!is_initialized_) {
+      83           0 :     return;
+      84             :   }
+      85             : 
+      86       23994 :   if (got_gnss_) {
+      87       23957 :     return;
+      88             :   }
+      89             : 
+      90          37 :   if (!std::isfinite(msg->latitude)) {
+      91           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->latitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+      92             :   }
+      93             : 
+      94          37 :   if (!std::isfinite(msg->longitude)) {
+      95           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->longitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+      96             :   }
+      97             : 
+      98          37 :   mrs_lib::UTM(msg->latitude, msg->longitude, &gnss_x_, &gnss_y_);
+      99          37 :   got_gnss_ = true;
+     100             : }
+     101             : /*//}*/
+     102             : 
+     103             : /*//{ process() */
+     104             : template <int n_measurements>
+     105       97272 : std::tuple<bool, bool> ProcTfToWorld<n_measurements>::process(measurement_t& measurement) {
+     106             : 
+     107       97272 :   if (!Processor<n_measurements>::enabled_) {
+     108           0 :     return {true, true};
+     109             :   }
+     110             : 
+     111       97272 :   if (!got_gnss_) {
+     112           1 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     113           1 :     return {false, false};
+     114             :   }
+     115             : 
+     116       97271 :   if (!is_gnss_offset_calculated_) {
+     117             : 
+     118          37 :     gnss_x_                    = (gnss_x_ - measurement(0)) - Processor<n_measurements>::ch_->world_origin.x;
+     119          37 :     gnss_y_                    = (gnss_y_ - measurement(1)) - Processor<n_measurements>::ch_->world_origin.y;
+     120          37 :     is_gnss_offset_calculated_ = true;
+     121             :   }
+     122             : 
+     123       97271 :   measurement(0) += gnss_x_;
+     124       97271 :   measurement(1) += gnss_y_;
+     125       97271 :   return {true, true};
+     126             : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ reset() */
+     130             : template <int n_measurements>
+     131           0 : void ProcTfToWorld<n_measurements>::reset() {
+     132           0 :   got_gnss_                  = false;
+     133           0 :   is_gnss_offset_calculated_ = false;
+     134           0 : }
+     135             : /*//}*/
+     136             : }  // namespace mrs_uav_state_estimators
+     137             : 
+     138             : #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..547cc3c590 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html @@ -0,0 +1,55 @@ + + + + + + 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 + + +
+ 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..c36e2d48ddbe16e7f9782c2a3dd82a0024d14e87 GIT binary patch literal 689 zcmV;i0#5yjP)v0{{R3&zjKJ0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp!uofd5*g)Qat11RYHFhk-x~tN)D!i`^E6Ah9Ea+9KyY%vB*UYHtkTKX4 z#r)W(T$7(uVnaga(JDg=~@!G6<$Qvj9xCJ0`<&ArqHB(!gCyT9OMIZo>54` zrW(`Q6{CJDH2B3GVj1UDfpqdpgrIK?A;{5y-)N7^3xG}qwKFB6OFW~;m=J9 zAB3)szOET1`$Ao(7>lk{3})j^qS%;OF&|?#Asro)U7yrP&+FSmy}hVoR>@Tz{x?Pv zU;ZAx;1qJIf17Cl&u;Xb;m^G>aB?SAZi#3wSlj_r@**U~8D_sb*Y726w?l2I0c09M zNE>5cO#GDkV*s>P{x9SEoubj!CZ>?KJ-^Q4)*{HwMbe+b@=!UB~n|2JC|-2QZC`~mp~ X<@_vk2t1d{00000NkvXXu0mjf+SEE; literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html new file mode 100644 index 0000000000..48a402a68b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.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/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:2023-12-06 07:59:45Functions: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<1>::getPrintName[abi:cxx11]() const21
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&)32
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const38
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&)39
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const41
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const44
+
+
+ + + +
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..81fcca9a5b --- /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:2023-12-06 07:59:45Functions: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&)32
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&)39
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const21
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const44
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const38
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const41
+
+
+ + + +
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..b0330536d3 --- /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:2023-12-06 07:59:45Functions: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          71 :   Processor(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          71 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30          71 :   }  // 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          85 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52          85 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58          59 : std::string Processor<n_measurements>::getPrintName() const {
+      59          59 :   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..5bde3e19271072329a6f5996af74c7f1cd4876bb GIT binary patch literal 436 zcmV;l0ZaagP)Ir7GwQk%4-g?oskOT)R(R$vYpnS+meZL^Ea=M0kJaxFSTc z32H$x9|fX3-#CC~#B?1rr;fx2&axu4p2Hm<8V4}X(NOjTn@>QGHiL*QhiTGJcM{QI zcEYd_=+;(>BBbidsb8hWiI0LEB;K?W4L$!}-{}$`SjR_JkDuK120ePg%olHM&$>x#~P3jjH@25qq;e;xFU5X@e8u zL5hL(9u|*xjz%}DKZxi46(Npi-`BmtM&E7WPzx=rL|%qa+ju<_$wWjHLqq#-Pk!}9 ej;eT@U+_NbxtO|J!@K?f0000 + + + + + + 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:2023-12-06 07:59:45Functions: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()35
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&)35
mrs_uav_state_estimators::garmin_agl::GarminAgl::start()35
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)457
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const42265
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)43361
+
+
+ + + +
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..cbbb3401e9 --- /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:2023-12-06 07:59:45Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()35
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&)35
mrs_uav_state_estimators::garmin_agl::GarminAgl::isConverged()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)43361
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)457
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()35
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const42265
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..040d2e4e4b --- /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:2023-12-06 07:59:45Functions: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          35 : void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          35 :   ch_ = ch;
+      17          35 :   ph_ = ph;
+      18          35 :   nh_ = nh;
+      19             : 
+      20          35 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      21             : 
+      22             :   // | --------------------- load parameters -------------------- |
+      23             : 
+      24          35 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          35 :   if (is_core_plugin_) {
+      27             : 
+      28          35 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      29          35 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          35 :   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          35 :   _update_timer_rate_       = 100;                                                                                          // TODO: parametrize
+      39          35 :   timer_update_             = nh.createTimer(ros::Rate(_update_timer_rate_), &GarminAgl::timerUpdate, this, false, false);  // not running after init
+      40          35 :   _check_health_timer_rate_ = 1;                                                                                            // TODO: parametrize
+      41          35 :   timer_check_health_       = nh.createTimer(ros::Rate(_check_health_timer_rate_), &GarminAgl::timerCheckHealth, this);
+      42             : 
+      43             :   // | --------------- subscribers initialization --------------- |
+      44          70 :   mrs_lib::SubscribeHandlerOptions shopts;
+      45          35 :   shopts.nh                 = nh;
+      46          35 :   shopts.node_name          = getPrintName();
+      47          35 :   shopts.no_message_timeout = ros::Duration(0.5);
+      48          35 :   shopts.threadsafe         = true;
+      49          35 :   shopts.autostart          = true;
+      50          35 :   shopts.queue_size         = 10;
+      51          35 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      52             : 
+      53             :   // | ---------------- publishers initialization --------------- |
+      54          35 :   ph_agl_height_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, Support::toSnakeCase(getName()) + "/agl_height", 10);
+      55          35 :   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          35 :   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          35 :   est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName(), is_core_plugin_);
+      65          35 :   est_agl_garmin_->initialize(nh, ch_, ph_);
+      66             : 
+      67          35 :   max_flight_z_ = est_agl_garmin_->getMaxFlightZ();
+      68             : 
+      69             :   // | ------------------ initialize published messages ------------------ |
+      70          35 :   agl_height_init_.header.frame_id     = ns_frame_id_;
+      71          35 :   agl_height_cov_init_.header.frame_id = ns_frame_id_;
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          35 :   if (changeState(INITIALIZED_STATE)) {
+      76          35 :     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          35 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84          35 : bool GarminAgl::start(void) {
+      85             : 
+      86             : 
+      87          35 :   if (isInState(READY_STATE)) {
+      88             : 
+      89             :     bool est_agl_garmin_start_successful;
+      90             : 
+      91          35 :     if (est_agl_garmin_->isStarted() || est_agl_garmin_->isRunning()) {
+      92           0 :       est_agl_garmin_start_successful = true;
+      93             :     } else {
+      94          35 :       est_agl_garmin_start_successful = est_agl_garmin_->start();
+      95             :     }
+      96             : 
+      97          35 :     if (est_agl_garmin_start_successful) {
+      98          35 :       timer_update_.start();
+      99          35 :       changeState(STARTED_STATE);
+     100          35 :       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       43361 : void GarminAgl::timerUpdate(const ros::TimerEvent &event) {
+     147             : 
+     148       43361 :   if (!isInitialized()) {
+     149           0 :     return;
+     150             :   }
+     151             : 
+     152       43361 :   const ros::Time time_now = ros::Time::now();
+     153             : 
+     154       86722 :   mrs_msgs::Float64Stamped agl_height = agl_height_init_;
+     155       43361 :   agl_height.header.stamp             = time_now;
+     156       43361 :   agl_height.value                    = est_agl_garmin_->getState(POSITION);
+     157             : 
+     158       86722 :   mrs_msgs::Float64ArrayStamped agl_height_cov;
+     159       43361 :   agl_height_cov.header.stamp = time_now;
+     160             : 
+     161       43361 :   const int n_states = 2;  // TODO this should be defined somewhere else
+     162       43361 :   agl_height_cov.values.resize(n_states * n_states);
+     163       43361 :   agl_height_cov.values.at(n_states * POSITION + POSITION) = est_agl_garmin_->getCovariance(POSITION);
+     164       43361 :   agl_height_cov.values.at(n_states * VELOCITY + VELOCITY) = est_agl_garmin_->getCovariance(VELOCITY);
+     165             : 
+     166       43361 :   mrs_lib::set_mutexed(mtx_agl_height_, agl_height, agl_height_);
+     167       43361 :   mrs_lib::set_mutexed(mtx_agl_height_cov_, agl_height_cov, agl_height_cov_);
+     168             : 
+     169       43361 :   publishAglHeight();
+     170       43361 :   publishCovariance();
+     171       43361 :   publishDiagnostics();
+     172             : }
+     173             : /*//}*/
+     174             : 
+     175             : /*//{ timerCheckHealth() */
+     176         457 : void GarminAgl::timerCheckHealth(const ros::TimerEvent &event) {
+     177             : 
+     178         457 :   if (!isInitialized()) {
+     179           0 :     return;
+     180             :   }
+     181             : 
+     182         457 :   if (isInState(INITIALIZED_STATE)) {
+     183             : 
+     184          40 :     if (est_agl_garmin_->isReady()) {
+     185          35 :       changeState(READY_STATE);
+     186          35 :       ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
+     187             :     } else {
+     188           5 :       ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     189           5 :       return;
+     190             :     }
+     191             :   }
+     192             : 
+     193             : 
+     194         452 :   if (isInState(STARTED_STATE)) {
+     195          38 :     ROS_INFO("[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     196             : 
+     197          38 :     if (est_agl_garmin_->isRunning()) {
+     198          35 :       ROS_INFO("[%s]: Subestimators converged", getPrintName().c_str());
+     199          35 :       changeState(RUNNING_STATE);
+     200             :     } else {
+     201           3 :       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       42265 : mrs_msgs::Float64Stamped GarminAgl::getUavAglHeight() const {
+     219       42265 :   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          35 : 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..44a7d2c9d16637b7d4f8db44d9baff875249aac4 GIT binary patch literal 1039 zcmV+q1n~QbP)JN zRA_WV7dW(!L$vFYzARj^pc5Q%yK<|2*nhk7GoCk9UxOq zjdq_pyxJdABYcgg$3Z4cF7ELy*&Sn*$K_t4xgLX&39;67AX_7`NXR8^*T@Aj;(VL# z@MD4D1rLaIJ)CUUV>|Zq%m414uQvd2t%_LpIv@b~0%#^9!DL3W=k(wrCD7hw>SCZa!Sa)&f78s~TSC^;}xhIEb?pX+E1RAH2u$`N30u6UF zx6(W^CIFcNBO_Un(Zytp(aO`+l(z%2@YuG47Je3D(}Z1QdfzLCdrXfMfV#ukqQZWQ zF)<)A*}v#gc6A@axn+|Axf;{@==?LGCU7cM3Z*ev>3li#@=|a&%im_N%id2W` z{1*4@Mxru8>LkVOx=;43xMJQFHD}5RJm23qH9YGyadiPl9FJ&Ec3sY)WQ`Tm>1ODI z{RS4RaSHtOu7$J(wJBMp*nFYbcKf@p6qpla-Qbs$)fS^BT=4W==XE$su6i-eID=sJ zI857(Tz@~#&Voi}*+V6Lr`^f-h%57ft~H|?8M(CzrW@Bch&zQ6UI(T#QUhLL12!~? zt8|aPy=M661wJZxONiI@QcuC&HSCOVZ)PYU98A%(l=xoZ7CXE~5b6fd6xU~g9j1-< zw$a9o;ce^eBT-r-aAbWm9%h3HCNUkrK|e-c`b%d8h%?N=M@-jyU1*vOtg7VSUEex} zT}7+uE_~vn!E3m&{BPBaIacKFI#pTGh+q|s$#_P??2l9xiaDM^8vSQo_9wV>y*OZ- zz8x3H&wr>n7;BP~0(nP!_aJxQKR+M^J&8lHPO77ibl(i1ixgjp_1CW{jGGFcThjN6 zo?*c>yT@Ie?YDhOh6R(G2v2URV4uc!{ZI7aVDw$xDolf{BL{gVR0e>51Bm-e&g<&F zBNdNCHZ&6TvCakY`Ma@}mh`Ou6*HjG$uo^1?OGahhGlZs^B-YB^lYwyEuH`X002ov JPDHLkV1lbp+?)Ua 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..06882fbcfb --- /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:2023-12-06 07:59:45Functions: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..c9dae27b9c --- /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:2023-12-06 07:59:45Functions: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..f6ea72dc4e --- /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:2023-12-06 07:59:45Functions: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..5b4525878d --- /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:2023-12-06 07:59:45Functions: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..fdc735fcee --- /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:2023-12-06 07:59:45Functions: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..2eb8940409 --- /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:2023-12-06 07:59:45Functions: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..ede6ff5b87 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + 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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.3 %
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::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::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&)74
mrs_uav_state_estimators::AltGeneric::isConverged()74
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)74
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)205
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const483
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const657
mrs_uav_state_estimators::AltGeneric::start()718
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const14205
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) const14205
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const48022
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const89376
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)89390
mrs_uav_state_estimators::AltGeneric::setDt(double const&)89402
mrs_uav_state_estimators::AltGeneric::getStates() const89420
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)95146
mrs_uav_state_estimators::AltGeneric::generateA()178809
mrs_uav_state_estimators::AltGeneric::generateB()178827
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const182757
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&)282390
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) const282396
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)282401
mrs_uav_state_estimators::AltGeneric::getState(int const&) const384057
+
+
+ + + +
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..a0facb1500 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + 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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.3 %
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&)74
mrs_uav_state_estimators::AltGeneric::isConverged()74
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)95146
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&)282390
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)282401
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)89390
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)74
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setDt(double const&)89402
mrs_uav_state_estimators::AltGeneric::start()718
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)205
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()178809
mrs_uav_state_estimators::AltGeneric::generateB()178827
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const657
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const182757
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const48022
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const483
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const89376
mrs_uav_state_estimators::AltGeneric::getState(int const&) const384057
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const14205
mrs_uav_state_estimators::AltGeneric::getStates() const89420
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) const282396
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) const14205
+
+
+ + + +
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..01bd9a4b66 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html @@ -0,0 +1,797 @@ + + + + + + + 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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.3 %
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          74 : void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          74 :   ch_ = ch;
+      17          74 :   ph_ = ph;
+      18             : 
+      19          74 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // clang-format off
+      22          74 :   dt_ = 0.01;
+      23          74 :   input_coeff_ = 10;
+      24          74 :   default_input_coeff_ = 10;
+      25             : 
+      26          74 :   generateA();
+      27          74 :   generateB();
+      28             : 
+      29          74 :   H_ <<
+      30          74 :     1, 0, 0;
+      31             : 
+      32             :   // clang-format on
+      33             : 
+      34             :   // | --------------- initialize parameter loader -------------- |
+      35             : 
+      36          74 :   if (is_core_plugin_) {
+      37             : 
+      38          74 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      39          74 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      40             :   }
+      41             : 
+      42          74 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      43             : 
+      44             :   // | --------------------- load parameters -------------------- |
+      45             : 
+      46          74 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      47          74 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      48          74 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      49          74 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      50          74 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      51          74 :   if (is_repredictor_enabled_) {
+      52           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      53             :   }
+      54             : 
+      55             :   // | --------------- corrections initialization --------------- |
+      56          74 :   ph->param_loader->loadParam("corrections", correction_names_);
+      57             : 
+      58         187 :   for (auto corr_name : correction_names_) {
+      59         113 :     corrections_.push_back(std::make_shared<Correction<alt_generic::n_measurements>>(
+      60       14431 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::ALTITUDE, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      61      282396 :         [this](const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      62      282396 :           return this->doCorrection(meas, R, state);
+      63             :         }));
+      64             :   }
+      65             : 
+      66          74 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      67             : 
+      68             :   // | ----------- initialize process noise covariance ---------- |
+      69          74 :   Q_ = Q_t::Zero();
+      70             :   double tmp_noise;
+      71          74 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      72          74 :   Q_(POSITION, POSITION) = tmp_noise;
+      73          74 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      74          74 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      75          74 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      76          74 :   Q_(ACCELERATION, ACCELERATION) = tmp_noise;
+      77             : 
+      78             :   // | ------- check if all parameters loaded successfully ------ |
+      79          74 :   if (!ph->param_loader->loadedSuccessfully()) {
+      80           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      81           0 :     ros::shutdown();
+      82             :   }
+      83             : 
+      84             :   // | ------------- initialize dynamic reconfigure ------------- |
+      85             :   drmgr_ =
+      86          74 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&AltGeneric::callbackReconfigure, this, _1, _2));
+      87          74 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      88          74 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      89          74 :   drmgr_->config.acc = Q_(ACCELERATION, ACCELERATION);
+      90          74 :   drmgr_->update_config(drmgr_->config);
+      91             : 
+      92             :   // | --------------- Kalman filter intialization -------------- |
+      93          74 :   const x_t        x0 = x_t::Zero();
+      94          74 :   const P_t        P0 = 1e3 * P_t::Identity();
+      95          74 :   const statecov_t sc0({x0, P0});
+      96          74 :   sc_ = sc0;
+      97             : 
+      98          74 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+      99          74 :   if (is_repredictor_enabled_) {
+     100             : 
+     101           0 :     for (int i = 0; i < alt_generic::n_states; i++) {
+     102           0 :       H_t H = H_t::Zero();
+     103           0 :       H(i)  = 1;
+     104           0 :       models_.push_back(std::make_shared<lkf_t>(A_, B_, H));
+     105             :     }
+     106             : 
+     107           0 :     const u_t       u0 = u_t::Zero();
+     108           0 :     const ros::Time t0 = ros::Time::now();
+     109           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_);
+     110             : 
+     111           0 :     setDt(1.0 / ch_->desired_uav_state_rate);
+     112             :   }
+     113             : 
+     114             :   // | ------------------ timers initialization ----------------- |
+     115          74 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &AltGeneric::timerUpdate, this);  // not running after init
+     116             :   /* timer_check_health_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &AltGeneric::timerCheckHealth, this); */
+     117             : 
+     118             :   // | --------------- subscribers initialization --------------- |
+     119             :   // subscriber to odometry
+     120         148 :   mrs_lib::SubscribeHandlerOptions shopts;
+     121          74 :   shopts.nh                 = nh;
+     122          74 :   shopts.node_name          = getPrintName();
+     123          74 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     124          74 :   shopts.threadsafe         = true;
+     125          74 :   shopts.autostart          = true;
+     126          74 :   shopts.queue_size         = 10;
+     127          74 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     128             : 
+     129          74 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     130             : 
+     131             :   // | ---------------- publishers initialization --------------- |
+     132          74 :   if (ch_->debug_topics.input) {
+     133          74 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     134             :   }
+     135          74 :   if (ch_->debug_topics.output) {
+     136          74 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     137             :   }
+     138          74 :   if (ch_->debug_topics.diag) {
+     139           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+     140             :   }
+     141             : 
+     142             :   // | ------------------ finish initialization ----------------- |
+     143             : 
+     144          74 :   if (changeState(INITIALIZED_STATE)) {
+     145          74 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+     146             :   } else {
+     147           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+     148             :   }
+     149          74 : }
+     150             : /*//}*/
+     151             : 
+     152             : /*//{ start() */
+     153         718 : bool AltGeneric::start(void) {
+     154             : 
+     155         718 :   if (isInState(READY_STATE)) {
+     156             :     /* timer_update_.start(); */
+     157          74 :     changeState(STARTED_STATE);
+     158          74 :     return true;
+     159             : 
+     160             :   } else {
+     161         644 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     162         644 :     return false;
+     163             :   }
+     164             : }
+     165             : /*//}*/
+     166             : 
+     167             : /*//{ pause() */
+     168           0 : bool AltGeneric::pause(void) {
+     169             : 
+     170           0 :   if (isInState(RUNNING_STATE)) {
+     171           0 :     changeState(STOPPED_STATE);
+     172           0 :     return true;
+     173             : 
+     174             :   } else {
+     175           0 :     return false;
+     176             :   }
+     177             : }
+     178             : /*//}*/
+     179             : 
+     180             : /*//{ reset() */
+     181           0 : bool AltGeneric::reset(void) {
+     182             : 
+     183           0 :   if (!isInitialized()) {
+     184           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     185           0 :     return false;
+     186             :   }
+     187             : 
+     188           0 :   changeState(STOPPED_STATE);
+     189             : 
+     190             :   // Initialize LKF state and covariance
+     191           0 :   const x_t        x0 = x_t::Zero();
+     192           0 :   const P_t        P0 = 1e6 * P_t::Identity();
+     193           0 :   const statecov_t sc0({x0, P0});
+     194           0 :   sc_ = sc0;
+     195             : 
+     196             :   // Instantiate the LKF itself
+     197           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     198           0 :   if (is_repredictor_enabled_) {
+     199             : 
+     200           0 :     const u_t       u0 = u_t::Zero();
+     201           0 :     const ros::Time t0 = ros::Time(0);
+     202           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_);
+     203             :   }
+     204             : 
+     205           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     206             : 
+     207           0 :   return true;
+     208             : }
+     209             : /*//}*/
+     210             : 
+     211             : /* timerUpdate() //{*/
+     212       95146 : void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
+     213             : 
+     214             : 
+     215       95146 :   if (!isInitialized()) {
+     216        5745 :     return;
+     217             :   }
+     218             : 
+     219       95115 :   switch (getCurrentSmState()) {
+     220             : 
+     221           0 :     case UNINITIALIZED_STATE: {
+     222           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     223           0 :       break;
+     224             :     }
+     225             : 
+     226        2845 :     case READY_STATE: {
+     227        2845 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     228        2845 :       break;
+     229             :     }
+     230             : 
+     231        2610 :     case INITIALIZED_STATE: {
+     232             :       // initialize the estimator with current corrections
+     233        2816 :       for (auto correction : corrections_) {
+     234        2737 :         auto res = correction->getProcessedCorrection();
+     235        2739 :         if (res) {
+     236         205 :           auto measurement_stamped = res.value();
+     237         205 :           setState(measurement_stamped.value(0), correction->getStateId());
+     238         205 :           ROS_INFO("[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     239             :         } else {
+     240        2532 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     241             :                             correction->getNamespacedName().c_str());
+     242        2535 :           return;
+     243             :         }
+     244             :       }
+     245          74 :       changeState(READY_STATE);
+     246          74 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     247          74 :       break;
+     248             :     }
+     249             : 
+     250          74 :     case STARTED_STATE: {
+     251          74 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     252             : 
+     253          74 :       if (isConverged()) {
+     254          74 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     255          74 :         changeState(RUNNING_STATE);
+     256             :       }
+     257          74 :       break;
+     258             :     }
+     259             : 
+     260       89577 :     case RUNNING_STATE: {
+     261      227128 :       for (auto correction : corrections_) {
+     262      137563 :         if (!correction->isHealthy()) {
+     263           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     264           0 :           changeState(ERROR_STATE);
+     265             :         }
+     266             :       }
+     267       89628 :       break;
+     268             :     }
+     269             : 
+     270           0 :     case STOPPED_STATE: {
+     271           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     272           0 :       break;
+     273             :     }
+     274             : 
+     275           0 :     case ERROR_STATE: {
+     276           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     277           0 :       ros::Time t_now = ros::Time::now();
+     278           0 :       if (is_error_state_first_time_) {
+     279           0 :         prev_time_in_error_state_  = t_now;
+     280           0 :         is_error_state_first_time_ = false;
+     281           0 :         error_state_duration_      = ros::Duration(0.0);
+     282             :       }
+     283           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     284             : 
+     285             : 
+     286             :       // check if all corrections are healthy now
+     287           0 :       bool all_corrections_healthy = true;
+     288           0 :       for (auto correction : corrections_) {
+     289           0 :         if (!correction->isHealthy()) {
+     290           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     291           0 :           all_corrections_healthy = false;
+     292             :         }
+     293             :       }
+     294             : 
+     295           0 :       if (all_corrections_healthy && innovation_ok_) {
+     296             :         // initialize the estimator again if corrections become healthy
+     297           0 :         if (error_state_duration_.toSec() > 5.0) {
+     298           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     299           0 :           changeState(INITIALIZED_STATE);
+     300           0 :           is_error_state_first_time_ = true;
+     301           0 :         }
+     302             :       } else {
+     303           0 :         is_error_state_first_time_ = true;
+     304             :       }
+     305             : 
+     306           0 :       prev_time_in_error_state_ = t_now;
+     307             : 
+     308           0 :       break;
+     309             :     }
+     310             :   }
+     311             : 
+     312       92664 :   if (sh_control_input_.newMsg()) {
+     313       43228 :     is_input_ready_ = true;
+     314             :   }
+     315             : 
+     316             :   // check age of input
+     317       92581 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     318           6 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     319             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     320           6 :     is_input_ready_ = false;
+     321             :   }
+     322             : 
+     323       92521 :   if (!isRunning() && !isStarted()) {
+     324        2918 :     return;
+     325             :   }
+     326             : 
+     327       89621 :   if (first_iter_) {
+     328          74 :     first_iter_ = false;
+     329          74 :     return;
+     330             :   }
+     331             : 
+     332       89547 :   double dt = (event.current_real - event.last_real).toSec();
+     333       89519 :   if (dt <= 0.0) {
+     334         217 :     return;
+     335             :   }
+     336             : 
+     337       89302 :   if (!is_repredictor_enabled_) {  // repredictor requires constant dt TODO: how to handle repredictor + variable rate?
+     338       89220 :     setDt(dt);
+     339             :   }
+     340             : 
+     341             :   // prediction step
+     342       89406 :   u_t       u;
+     343       89225 :   ros::Time input_stamp;
+     344       89275 :   if (is_input_ready_) {
+     345       60043 :     mrs_msgs::EstimatorInputConstPtr msg            = sh_control_input_.getMsg();
+     346       60135 :     const tf2::Vector3               des_acc_global = getAccGlobal(msg, 0);  // we don't care about heading
+     347       60220 :     input_stamp                                     = msg->header.stamp;
+     348       59956 :     setInputCoeff(default_input_coeff_);
+     349       60145 :     u(0) = des_acc_global.getZ();
+     350             :   } else {
+     351       29077 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     352       29150 :     input_stamp = ros::Time::now();
+     353       29170 :     setInputCoeff(0);
+     354       29129 :     u = u_t::Zero();
+     355             :   }
+     356             : 
+     357             :   // go through available corrections and apply them
+     358             :   /* for (auto correction : corrections_) { */
+     359             :   /*   auto res = correction->getProcessedCorrection(); */
+     360             :   /*   if (res) { */
+     361             :   /*     auto measurement_stamped = res.value(); */
+     362             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     363             :   /*   } */
+     364             :   /* } */
+     365             : 
+     366       88965 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     367       88951 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     368             : 
+     369             :   try {
+     370             :     // Apply the prediction step
+     371      178221 :     std::scoped_lock lock(mutex_lkf_);
+     372       89111 :     if (is_repredictor_enabled_) {
+     373           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, lkf_);
+     374           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     375             :     } else {
+     376       89111 :       sc = lkf_->predict(sc, u, Q, dt_);
+     377             :     }
+     378             :   }
+     379           0 :   catch (const std::exception &e) {
+     380             :     // In case of error, alert the user
+     381           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     382             :   }
+     383             : 
+     384       88651 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     385             : 
+     386             :   // publishing
+     387       89070 :   publishInput(u, input_stamp);
+     388       89414 :   publishOutput();
+     389       89421 :   publishDiagnostics();
+     390             : }
+     391             : /*//}*/
+     392             : 
+     393             : /*//{ timerCheckHealth() */
+     394           0 : void AltGeneric::timerCheckHealth(const ros::TimerEvent &event) {
+     395             : 
+     396           0 :   if (!isInitialized()) {
+     397           0 :     return;
+     398             :   }
+     399             : 
+     400           0 :   switch (getCurrentSmState()) {
+     401             : 
+     402           0 :     case UNINITIALIZED_STATE: {
+     403           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     404           0 :       break;
+     405             :     }
+     406             : 
+     407           0 :     case READY_STATE: {
+     408           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     409           0 :       break;
+     410             :     }
+     411             : 
+     412           0 :     case INITIALIZED_STATE: {
+     413             : 
+     414             :       // initialize the estimator with current corrections
+     415           0 :       for (auto correction : corrections_) {
+     416           0 :         auto res = correction->getProcessedCorrection();
+     417           0 :         if (res) {
+     418           0 :           auto measurement_stamped = res.value();
+     419           0 :           setState(measurement_stamped.value(0), correction->getStateId());
+     420           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     421             :         } else {
+     422           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     423             :                             correction->getNamespacedName().c_str());
+     424           0 :           return;
+     425             :         }
+     426             :       }
+     427           0 :       changeState(READY_STATE);
+     428           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     429           0 :       break;
+     430             :     }
+     431             : 
+     432           0 :     case STARTED_STATE: {
+     433           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     434             : 
+     435           0 :       if (isConverged()) {
+     436           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     437           0 :         changeState(RUNNING_STATE);
+     438             :       }
+     439           0 :       break;
+     440             :     }
+     441             : 
+     442           0 :     case RUNNING_STATE: {
+     443           0 :       for (auto correction : corrections_) {
+     444           0 :         if (!correction->isHealthy()) {
+     445           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     446           0 :           changeState(ERROR_STATE);
+     447             :         }
+     448             :       }
+     449           0 :       break;
+     450             :     }
+     451             : 
+     452           0 :     case STOPPED_STATE: {
+     453           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     454           0 :       break;
+     455             :     }
+     456             : 
+     457           0 :     case ERROR_STATE: {
+     458           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     459           0 :       bool all_corrections_healthy = true;
+     460           0 :       for (auto correction : corrections_) {
+     461           0 :         if (!correction->isHealthy()) {
+     462           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     463           0 :           all_corrections_healthy = false;
+     464             :         }
+     465             :       }
+     466             :       // initialize the estimator again if corrections become healthy
+     467           0 :       if (all_corrections_healthy) {
+     468           0 :         changeState(INITIALIZED_STATE);
+     469             :       }
+     470           0 :       break;
+     471             :     }
+     472             :   }
+     473             : 
+     474           0 :   if (sh_control_input_.newMsg()) {
+     475           0 :     is_input_ready_ = true;
+     476             :   }
+     477             : 
+     478             :   // check age of input
+     479           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     480           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     481             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     482           0 :     is_input_ready_ = false;
+     483             :   }
+     484             : }
+     485             : /*//}*/
+     486             : 
+     487             : /*//{ doCorrection() */
+     488      282401 : void AltGeneric::doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     489      282401 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     490      282255 : }
+     491             : /*//}*/
+     492             : 
+     493             : /*//{ doCorrection() */
+     494      282390 : void AltGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     495             : 
+     496      282390 :   if (!isInitialized()) {
+     497         925 :     return;
+     498             :   }
+     499             : 
+     500             :   // we do not want to perform corrections until the estimator is initialized
+     501      282278 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     502         925 :     return;
+     503             :   }
+     504             : 
+     505             :   // for position state check the innovation
+     506      281253 :   if (H_idx == POSITION) {
+     507      182314 :     std::scoped_lock lock(mtx_innovation_);
+     508             : 
+     509      182350 :     is_mitigating_jump_ = false;
+     510      182416 :     innovation_(0)      = z(0) - getState(POSITION);
+     511             : 
+     512      182308 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     513           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), pos_innovation_limit_);
+     514           0 :       innovation_ok_ = false;
+     515           0 :       switch (exc_innovation_action_) {
+     516           0 :         case ExcInnoAction_t::ELAND: {
+     517           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", ros::this_node::getName().c_str());
+     518           0 :           changeState(ERROR_STATE);
+     519           0 :           break;
+     520             :         }
+     521           0 :         case ExcInnoAction_t::SWITCH: {
+     522           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", ros::this_node::getName().c_str());
+     523           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?
+     524           0 :           changeState(ERROR_STATE);
+     525           0 :           break;
+     526             :         }
+     527           0 :         case ExcInnoAction_t::MITIGATE: {
+     528           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", ros::this_node::getName().c_str());
+     529           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?
+     530           0 :           is_mitigating_jump_ = true;
+     531           0 :           setState(z(0), POSITION);
+     532           0 :           break;
+     533             :         }
+     534           0 :         case ExcInnoAction_t::NONE: {
+     535           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", ros::this_node::getName().c_str());
+     536           0 :           break;
+     537             :         }
+     538             :       }
+     539             :     }
+     540      182323 :     innovation_ok_ = true;
+     541             :   }
+     542             : 
+     543      281244 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     544             :   try {
+     545             :     // Apply the correction step
+     546             :     {
+     547      561336 :       std::scoped_lock lock(mutex_lkf_);
+     548      281366 :       H_        = H_t::Zero();
+     549      281327 :       H_(H_idx) = 1;
+     550      281046 :       lkf_->H   = H_;
+     551      280717 :       if (is_repredictor_enabled_) {
+     552             : 
+     553           0 :         lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     554             :       } else {
+     555      280717 :         sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     556             :       }
+     557             :     }
+     558             :   }
+     559           0 :   catch (const std::exception &e) {
+     560             :     // In case of error, alert the user
+     561           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     562             :   }
+     563             : 
+     564      279997 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     565             : }  // namespace mrs_uav_state_estimators
+     566             : /*//}*/
+     567             : 
+     568             : /*//{ isConverged() */
+     569          74 : bool AltGeneric::isConverged() {
+     570             : 
+     571             :   // TODO: check convergence by rate of change of determinant
+     572             : 
+     573          74 :   return true;
+     574             : }
+     575             : /*//}*/
+     576             : 
+     577             : /*//{ getState() */
+     578       14205 : double AltGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     579       14205 :   return getState(stateIdToIndex(state_id_in, 0));
+     580             : }
+     581             : 
+     582      384057 : double AltGeneric::getState(const int &state_idx_in) const {
+     583      384057 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     584             : }
+     585             : /*//}*/
+     586             : 
+     587             : /*//{ setState() */
+     588           0 : void AltGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     589           0 :   setState(state_in, stateIdToIndex(state_id_in, 0));
+     590           0 : }
+     591             : 
+     592         205 : void AltGeneric::setState(const double &state_in, const int &state_idx_in) {
+     593         205 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     594         205 : }
+     595             : /*//}*/
+     596             : 
+     597             : /*//{ getStates() */
+     598       89420 : AltGeneric::states_t AltGeneric::getStates(void) const {
+     599      178796 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     600             : }
+     601             : /*//}*/
+     602             : 
+     603             : /*//{ setStates() */
+     604           0 : void AltGeneric::setStates(const states_t &states_in) {
+     605           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     606           0 : }
+     607             : /*//}*/
+     608             : 
+     609             : /*//{ getCovariance() */
+     610           0 : double AltGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     611           0 :   return getCovariance(stateIdToIndex(state_id_in, 0));
+     612             : }
+     613             : 
+     614      182757 : double AltGeneric::getCovariance(const int &state_idx_in) const {
+     615      182757 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     616             : }
+     617             : /*//}*/
+     618             : 
+     619             : /*//{ getCovarianceMatrix() */
+     620       89376 : AltGeneric::covariance_t AltGeneric::getCovarianceMatrix(void) const {
+     621      178711 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     622             : }
+     623             : /*//}*/
+     624             : 
+     625             : /*//{ setCovarianceMatrix() */
+     626           0 : void AltGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     627           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     628           0 : }
+     629             : /*//}*/
+     630             : 
+     631             : /*//{ getInnovation() */
+     632       48022 : double AltGeneric::getInnovation(const int &state_idx) const {
+     633       48022 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     634             : }
+     635             : 
+     636           0 : double AltGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     637           0 :   return getInnovation(stateIdToIndex(0, 0));
+     638             : }
+     639             : /*//}*/
+     640             : 
+     641             : /*//{ setDt() */
+     642       89402 : void AltGeneric::setDt(const double &dt) {
+     643       89402 :   dt_ = dt;
+     644       89402 :   generateA();
+     645       89224 :   generateB();
+     646      178553 :   std::scoped_lock lock(mutex_lkf_);
+     647       89232 :   lkf_->A = A_;
+     648       89292 :   lkf_->B = B_;
+     649       89215 : }
+     650             : /*//}*/
+     651             : 
+     652             : /*//{ setInputCoeff() */
+     653       89390 : void AltGeneric::setInputCoeff(const double &input_coeff) {
+     654       89390 :   input_coeff_ = input_coeff;
+     655       89390 :   generateA();
+     656       89164 :   generateB();
+     657      178195 :   std::scoped_lock lock(mutex_lkf_);
+     658       89030 :   lkf_->A = A_;
+     659       89119 :   lkf_->B = B_;
+     660       89161 : }
+     661             : /*//}*/
+     662             : 
+     663             : /*//{ generateA() */
+     664      178809 : void AltGeneric::generateA() {
+     665             : 
+     666             :   // clang-format off
+     667      178809 :     A_ <<
+     668      178231 :       1, dt_, 0.5 * dt_ * dt_,
+     669      178367 :       0, 1, dt_,
+     670      178533 :       0, 0, 1-(input_coeff_ * dt_);
+     671             :   // clang-format on
+     672      178285 : }
+     673             : /*//}*/
+     674             : 
+     675             : /*//{ generateB() */
+     676      178827 : void AltGeneric::generateB() {
+     677             : 
+     678             :   // clang-format off
+     679      178827 :     B_ <<
+     680             :       0,
+     681      178031 :       0,
+     682      178133 :       (input_coeff_ * dt_);
+     683             :   // clang-format on
+     684      178086 : }
+     685             : /*//}*/
+     686             : 
+     687             : /*//{ callbackReconfigure() */
+     688          74 : void AltGeneric::callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     689             : 
+     690          74 :   if (!isInitialized()) {
+     691          74 :     return;
+     692             :   }
+     693             : 
+     694           0 :   Q_t Q;
+     695           0 :   Q(POSITION, POSITION)         = config.pos;
+     696           0 :   Q(VELOCITY, VELOCITY)         = config.vel;
+     697           0 :   Q(ACCELERATION, ACCELERATION) = config.acc;
+     698           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     699             : }
+     700             : /*//}*/
+     701             : 
+     702             : /*//{ getNamespacedName() */
+     703         483 : std::string AltGeneric::getNamespacedName() const {
+     704         966 :   return parent_state_est_name_ + "/" + getName();
+     705             : }
+     706             : /*//}*/
+     707             : 
+     708             : /*//{ getPrintName() */
+     709         657 : std::string AltGeneric::getPrintName() const {
+     710        1314 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     711             : }
+     712             : /*//}*/
+     713             : };  // 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..80f7be6c43 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html @@ -0,0 +1,199 @@ + + + + + + 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 + + +
+ 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..6ee805116614304acd38f3072d79ca11c3ef5173 GIT binary patch literal 2791 zcmVn zjQ7jWH@+{K&Cj?GH>o^&cYYD7*&au{r613D`4)#j#IZ@Q&u9J=!}#x#9}JUn4AaM$ zkGT|3zqB9ooc=t%%oowm2!w3V(Icb42JKM?*aYq-&y|4srDp}F{fs+gd*xFsyimKM zjd~?xFoNY8yY+bjWt*?TdP3qO46hpf1``n==696GY#K5!x?sq z@lQl>fp{1bQv=KmF#((zD;+15XQWe3Sj_xSMLI6aCJ!6GSeId07uD#Xc&1!Qsd5(b zSObDc6_nC5Ic9l&A^Bs7q|q5a{L)wgj?*actpF@p#?LrqV8;Kg*rS6}V12#!aid-* zdpo|dawDM#n$j>n?{<96j7fZ?S-Uy2f4oNE$e0uubB5hvF(cR> z>;AXfy8nj^kNfPtfdCt}=T0Mq1rn?joHGtYr8A;U!KQ!4r$Ur;%40?-dI7;UwZ)7T zieBkKQiI7#jr|cT%+Z7hGZH{BHv(W(g^KGQJbMK)YOJT?Jl_KqRtvS~wR-7YXLjAO z)~@SHj`Wr4$PTzeMwqH{IW=6?h7LwRZg~ABfI5BEYpYv%U&Zaf+TgkfdpvB*{>dgc zM*<8#0l?BxvV#f}n2~UW-RHe77YSnuuQ?b1dWvV`h!vidhiH<0ZmKe2 zVa-Y>Dyb_n4|2)^W(;qZlb@5}-dGii z4nwNgpg)GS(Qvl3kqLg#rxNKMC)U}e#g&oEm7AYzJZXE>qjWTR%pr<1>C<5ZW*W2) zPWr29nUap-lRMgMJPgjzQOW9Q_-K-kQ%`m#y0uL}8jQ+>>dF@C1a~5ka^5hH*sC*o zeA;_djy=9shX9z2AeOTUNX|!kBJHuoaJ{8aD_P@AJu%c`MpeklI-^r92S{ejU0^a} z!zm+xwiS@}>`^T}ap{(2(c7NCUMNI?2s478f7p63{Tn(*Z{Uo2DCM?C9;16bhEH#y z{+U89TaO|sAUGfbU_44!8J%JA({lJIC=<-+Pvn7hcq!U}JTBQWo1!PBD(~M? zSj1V=${v+VJ5>M`kPn6|%jHjo`0ua?uK`daxR2d6A4J;zE@2$Em~Km{Eq zC9}^UQ^&{eG1T9ZXPxlK0b?N$dP(>h{o-86UINbTATn0>U_POmmCAtmpt*(&(7?w* z0|e~>HF0jSG>C;MXN19QHlUSeY{xT`PJWzR=uC6Av07YMMrWo_1T%e!Dzr>9!$1;a zvMg-On9iUEV6nDI(wRm9SjlKA3HfU{qqjYZ%;Mo7;gcsg(C%PbkwaYoO8&J6)Er5@ zTBsoS^_)Qmdn?>44)!gb?#MfS@?ZyKs<>2;S*f=j0au(F=BD<5G)CaJ;H8IvSw1u_ z05=`rN`jRfP(^V{9?xPL|Evg5HS{bTjwm2CQ;Z=KQYg|wUD9KV5;7@05%FN+&4Z4L zjM#aT;$=vh@miCj9y!wBuTvT!@~kMopL$Z(C>IPJ56p9#6Y@aX$j@$blTsAVB`su> z<|hPqaJ9r5B~N?h4lV3rp+d^Kaw(VF@Ef}9XxJe+7O2iu?@+%X5I=00vu2G{2_Rzt zEhetwiXAmK?B1;JG|obgSu7W&S)vI7%oq^_5aq>Gl^3yK+iz$2%cJTYv5=4+`-R*bh0Nznj#yQPCI zH{nL;Chxwn*rq0FV5?@YtK!N5HjKEwG3~DA5XmN@vE@-XB(}VG^rvz9nnM(#c2nsb z;eAnO!T+ZC`I8)J%JtG_&Tl}bj!##j5_R}^ReK>v0x#W@R&-{WIlW3i+djNvmJ8++ z`d!$(S!5wdBLisD0TR1-QASg9zVb81lc~te@=G|A z$?f)+R7wBU;wmHeDMq-(H$sF^AT1cl((DO(_^8>tyyBy)u;XZWt+Yumw?rT6iVq+I zK?3I^4y;SDf^T*t2Iw(Ngj}9ctQ7W==kco&%~+V}85S-%(uBs0FUOtD0kpCS3Gk0# zZDNO2>UA@H@0)#p1GGi*j|3Wghp3 zDLYh6sTM#1a5~y;%lC>-ERB2JF-A1QzOe4h5z|;3P3gKJK%wO-*~fUbsc|Hw8~|Eg zoRPKnXvb^ZquvkK9PM2#F^d{&f#l#nu9DnE}vLUid(Onp$PgU8y`L74_C`NWeFJZ{alMwaP#QrZ%kQ^~IX- zm)0~5i4Q6geq-R%YN+_=YVo}V+E?gD>5CPvLK-XbSnTl&!;0DsVC(GYaKmtRr*DDf^WzwdXtjv9Jc%%|eAodiAr8`d_A_{B + + + + + + 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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.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.cpp +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
<unnamed>56.3 %209 / 37173.3 %22 / 30
+
+
+ + + + +
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..020914ffff --- /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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.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.cpp +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
<unnamed>56.3 %209 / 37173.3 %22 / 30
+
+
+ + + + +
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..bee8f80d52 --- /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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.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.cpp +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
<unnamed>56.3 %209 / 37173.3 %22 / 30
+
+
+ + + + +
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..96ffa5aa04 --- /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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.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.cpp +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
+
+
+ + + + +
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..b468e3857e --- /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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.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.cpp +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
+
+
+ + + + +
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..7fcc1757bc --- /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:20937156.3 %
Date:2023-12-06 07:59:45Functions:223073.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.cpp +
56.3%56.3%
+ 		56.3 %209 / 37173.3 %22 / 30
+
+
+ + + + +
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..656e703133 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html @@ -0,0 +1,204 @@ + + + + + + + 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:03350.0 %
Date:2023-12-06 07:59:45Functions:0310.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::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
+
+
+ + + +
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..8939d13e1f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + 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:03350.0 %
Date:2023-12-06 07:59:45Functions:0310.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::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
+
+
+ + + +
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..85b9c90786 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html @@ -0,0 +1,754 @@ + + + + + + + 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:03350.0 %
Date:2023-12-06 07:59:45Functions:0310.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             :   // clang-format on
+      33             : 
+      34             :   // | --------------- initialize parameter loader -------------- |
+      35             : 
+      36           0 :   if (is_core_plugin_) {
+      37             : 
+      38           0 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      39           0 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      40             :   }
+      41             : 
+      42           0 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      43             : 
+      44             :   // | --------------------- load parameters -------------------- |
+      45           0 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      46           0 :   ph->param_loader->loadParam("position_innovation_limit", pos_innovation_limit_);
+      47           0 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      48           0 :   if (is_repredictor_enabled_) {
+      49           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      50             :   }
+      51             : 
+      52             :   // | --------------- corrections initialization --------------- |
+      53           0 :   ph->param_loader->loadParam("corrections", correction_names_);
+      54             : 
+      55           0 :   for (auto corr_name : correction_names_) {
+      56           0 :     corrections_.push_back(std::make_shared<Correction<hdg_generic::n_measurements>>(
+      57           0 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::HEADING, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      58           0 :         [this](const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      59           0 :           return this->doCorrection(meas, R, state);
+      60             :         }));
+      61             :   }
+      62             : 
+      63           0 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      64             : 
+      65             :   // | ----------- initialize process noise covariance ---------- |
+      66           0 :   Q_ = Q_t::Zero();
+      67             :   double tmp_noise;
+      68           0 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      69           0 :   Q_(POSITION, POSITION) = tmp_noise;
+      70           0 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      71           0 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      72             : 
+      73             :   // | ------- check if all parameters loaded successfully ------ |
+      74           0 :   if (!ph->param_loader->loadedSuccessfully()) {
+      75           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      76           0 :     ros::shutdown();
+      77             :   }
+      78             : 
+      79             :   // | ------------- initialize dynamic reconfigure ------------- |
+      80             :   drmgr_ =
+      81           0 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&HdgGeneric::callbackReconfigure, this, _1, _2));
+      82           0 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      83           0 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      84           0 :   drmgr_->update_config(drmgr_->config);
+      85             : 
+      86             :   // | --------------- Kalman filter intialization -------------- |
+      87           0 :   const x_t        x0 = x_t::Zero();
+      88           0 :   const P_t        P0 = 1e3 * P_t::Identity();
+      89           0 :   const statecov_t sc0({x0, P0});
+      90           0 :   sc_ = sc0;
+      91             : 
+      92           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+      93           0 :   if (is_repredictor_enabled_) {
+      94             : 
+      95           0 :     for (int i = 0; i < hdg_generic::n_states; i++) {
+      96           0 :       H_t H = H_t::Zero();
+      97           0 :       H(i)  = 1;
+      98           0 :       models_.push_back(std::make_shared<lkf_t>(A_, B_, H));
+      99             :     }
+     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<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, 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<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, 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 requires constant dt
+     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 :     setInputCoeff(default_input_coeff_);
+     328           0 :     u(0) = sh_control_input_.getMsg()->control_hdg_rate;
+     329             :   } else {
+     330           0 :     input_stamp = ros::Time::now();
+     331           0 :     setInputCoeff(0);
+     332           0 :     u = u_t::Zero();
+     333             :   }
+     334             : 
+     335           0 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     336           0 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     337             :   try {
+     338             :     // Apply the prediction step
+     339           0 :     std::scoped_lock lock(mutex_lkf_);
+     340           0 :     if (is_repredictor_enabled_) {
+     341           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, lkf_);
+     342           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     343             :     } else {
+     344           0 :       sc = lkf_->predict(sc, u, Q, dt_);
+     345             :     }
+     346             :   }
+     347           0 :   catch (const std::exception &e) {
+     348             :     // In case of error, alert the user
+     349           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     350             :   }
+     351             : 
+     352           0 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     353             : 
+     354           0 :   if (!isError()) {
+     355           0 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, sc.x(POSITION), last_valid_hdg_);
+     356             :   }
+     357             : 
+     358             :   // publishing
+     359           0 :   publishInput(u, input_stamp);
+     360           0 :   publishOutput();
+     361           0 :   publishDiagnostics();
+     362             : }
+     363             : /*//}*/
+     364             : 
+     365             : /*//{ timerCheckHealth() */
+     366           0 : void HdgGeneric::timerCheckHealth(const ros::TimerEvent &event) {
+     367             : 
+     368           0 :   if (!isInitialized()) {
+     369           0 :     return;
+     370             :   }
+     371             : 
+     372           0 :   switch (getCurrentSmState()) {
+     373             : 
+     374           0 :     case UNINITIALIZED_STATE: {
+     375           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     376           0 :       break;
+     377             :     }
+     378             : 
+     379           0 :     case READY_STATE: {
+     380           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     381           0 :       break;
+     382             :     }
+     383             : 
+     384           0 :     case INITIALIZED_STATE: {
+     385             :       // initialize the estimator with current corrections
+     386           0 :       for (auto correction : corrections_) {
+     387           0 :         auto res = correction->getProcessedCorrection();
+     388           0 :         if (res) {
+     389           0 :           auto measurement_stamped = res.value();
+     390           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     391           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X));
+     392             :         } else {
+     393           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     394             :                             correction->getNamespacedName().c_str());
+     395           0 :           return;
+     396             :         }
+     397             :       }
+     398           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     399           0 :       changeState(READY_STATE);
+     400           0 :       break;
+     401             :     }
+     402             : 
+     403           0 :     case STARTED_STATE: {
+     404           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     405           0 :       if (isConverged()) {
+     406           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     407           0 :         changeState(RUNNING_STATE);
+     408             :       }
+     409           0 :       break;
+     410             :     }
+     411             : 
+     412           0 :     case RUNNING_STATE: {
+     413           0 :       for (auto correction : corrections_) {
+     414           0 :         if (!correction->isHealthy()) {
+     415           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     416           0 :           changeState(ERROR_STATE);
+     417             :         }
+     418             :       }
+     419           0 :       break;
+     420             :     }
+     421             : 
+     422           0 :     case STOPPED_STATE: {
+     423           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     424           0 :       break;
+     425             :     }
+     426             : 
+     427           0 :     case ERROR_STATE: {
+     428           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     429           0 :       bool all_corrections_healthy = true;
+     430           0 :       for (auto correction : corrections_) {
+     431           0 :         if (!correction->isHealthy()) {
+     432           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     433           0 :           all_corrections_healthy = false;
+     434             :         }
+     435             :       }
+     436             :       // initialize the estimator again if corrections become healthy
+     437           0 :       if (all_corrections_healthy) {
+     438           0 :         changeState(INITIALIZED_STATE);
+     439             :       }
+     440           0 :       break;
+     441             :     }
+     442             :   }
+     443             : 
+     444           0 :   if (sh_control_input_.newMsg()) {
+     445           0 :     is_input_ready_ = true;
+     446             :   }
+     447             : 
+     448             :   // check age of input
+     449           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     450           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     451             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     452           0 :     is_input_ready_ = false;
+     453             :   }
+     454             : }
+     455             : /*//}*/
+     456             : 
+     457             : /*//{ doCorrection() */
+     458           0 : void HdgGeneric::doCorrection(const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     459           0 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     460           0 : }
+     461             : /*//}*/
+     462             : 
+     463             : /*//{ doCorrection() */
+     464           0 : void HdgGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     465             : 
+     466           0 :   if (!isInitialized()) {
+     467           0 :     return;
+     468             :   }
+     469             : 
+     470             :   // copy measurement as we might need to modify it (unwrap)
+     471           0 :   z_t meas = z;
+     472             : 
+     473             :   // we do not want to perform corrections until the estimator is initialized
+     474           0 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     475           0 :     return;
+     476             :   }
+     477             : 
+     478           0 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     479             : 
+     480             :   // for position state check the innovation
+     481           0 :   if (H_idx == POSITION) {
+     482             : 
+     483             :     // unwrap the hdg measurement wrt current state
+     484           0 :     meas(POSITION) = mrs_lib::geometry::radians::unwrap(meas(POSITION), sc.x(POSITION));
+     485             : 
+     486           0 :     std::scoped_lock lock(mtx_innovation_);
+     487             : 
+     488           0 :     innovation_(0) = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(meas(0)), mrs_lib::geometry::radians(sc.x(POSITION)));
+     489             : 
+     490           0 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     491           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - hdg: %.2f", getPrintName().c_str(), innovation_(0));
+     492           0 :       innovation_ok_ = false;
+     493           0 :       changeState(ERROR_STATE);
+     494             :     }
+     495             :   }
+     496             : 
+     497             :   try {
+     498             :     // Apply the correction step
+     499             :     {
+     500           0 :       std::scoped_lock lock(mutex_lkf_);
+     501           0 :       H_        = H_t::Zero();
+     502           0 :       H_(H_idx) = 1;
+     503           0 :       lkf_->H   = H_;
+     504           0 :       if (is_repredictor_enabled_) {
+     505           0 :         lkf_rep_->addMeasurement(meas, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     506             :       } else {
+     507           0 :         sc = lkf_->correct(sc, meas, R_t::Ones() * R);
+     508             :       }
+     509             :     }
+     510           0 :     innovation_ok_ = true;
+     511             :   }
+     512           0 :   catch (const std::exception &e) {
+     513             :     // In case of error, alert the user
+     514           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     515             :   }
+     516             : 
+     517           0 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     518             : }
+     519             : /*//}*/
+     520             : 
+     521             : /*//{ isConverged() */
+     522           0 : bool HdgGeneric::isConverged() {
+     523             : 
+     524             :   // TODO: check convergence by rate of change of determinant
+     525             : 
+     526           0 :   return true;
+     527             : }
+     528             : /*//}*/
+     529             : 
+     530             : /*//{ getState() */
+     531           0 : double HdgGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     532           0 :   return getState(stateIdToIndex(state_id_in, 0));
+     533             : }
+     534             : 
+     535           0 : double HdgGeneric::getState(const int &state_idx_in) const {
+     536           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     537             : }
+     538             : /*//}*/
+     539             : 
+     540             : /*//{ setState() */
+     541           0 : void HdgGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     542           0 :   setState(state_in, stateIdToIndex(state_id_in, 0));
+     543           0 : }
+     544             : 
+     545           0 : void HdgGeneric::setState(const double &state_in, const int &state_idx_in) {
+     546           0 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     547           0 : }
+     548             : /*//}*/
+     549             : 
+     550             : /*//{ getStates() */
+     551           0 : HdgGeneric::states_t HdgGeneric::getStates(void) const {
+     552           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     553             : }
+     554             : /*//}*/
+     555             : 
+     556             : /*//{ setStates() */
+     557           0 : void HdgGeneric::setStates(const states_t &states_in) {
+     558           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     559           0 : }
+     560             : /*//}*/
+     561             : 
+     562             : /*//{ getCovariance() */
+     563           0 : double HdgGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     564           0 :   return getCovariance(stateIdToIndex(state_id_in, 0));
+     565             : }
+     566             : 
+     567           0 : double HdgGeneric::getCovariance(const int &state_idx_in) const {
+     568           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     569             : }
+     570             : /*//}*/
+     571             : 
+     572             : /*//{ getCovarianceMatrix() */
+     573           0 : HdgGeneric::covariance_t HdgGeneric::getCovarianceMatrix(void) const {
+     574           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     575             : }
+     576             : /*//}*/
+     577             : 
+     578             : /*//{ setCovarianceMatrix() */
+     579           0 : void HdgGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     580           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     581           0 : }
+     582             : /*//}*/
+     583             : 
+     584             : /*//{ getInnovation() */
+     585           0 : double HdgGeneric::getInnovation(const int &state_idx) const {
+     586           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     587             : }
+     588             : 
+     589           0 : double HdgGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     590           0 :   return getInnovation(stateIdToIndex(0, 0));
+     591             : }
+     592             : /*//}*/
+     593             : 
+     594             : /*//{ setDt() */
+     595           0 : void HdgGeneric::setDt(const double &dt) {
+     596           0 :   dt_ = dt;
+     597           0 :   generateA();
+     598           0 :   generateB();
+     599           0 :   std::scoped_lock lock(mutex_lkf_);
+     600           0 :   lkf_->A = A_;
+     601           0 :   lkf_->B = B_;
+     602           0 : }
+     603             : /*//}*/
+     604             : 
+     605             : /*//{ setInputCoeff() */
+     606           0 : void HdgGeneric::setInputCoeff(const double &input_coeff) {
+     607           0 :   input_coeff_ = input_coeff;
+     608           0 :   generateA();
+     609           0 :   generateB();
+     610           0 :   std::scoped_lock lock(mutex_lkf_);
+     611           0 :   lkf_->A = A_;
+     612           0 :   lkf_->B = B_;
+     613           0 : }
+     614             : /*//}*/
+     615             : 
+     616             : /*//{ generateA() */
+     617           0 : void HdgGeneric::generateA() {
+     618             : 
+     619             :   // clang-format off
+     620           0 :     A_ <<
+     621           0 :       1, dt_,
+     622           0 :       0, 1-(input_coeff_ * dt_);
+     623             :   // clang-format on
+     624           0 : }
+     625             : /*//}*/
+     626             : 
+     627             : /*//{ generateB() */
+     628           0 : void HdgGeneric::generateB() {
+     629             : 
+     630             :   // clang-format off
+     631           0 :     B_ <<
+     632             :       0,
+     633           0 :       (input_coeff_ * dt_);
+     634             :   // clang-format on
+     635           0 : }
+     636             : /*//}*/
+     637             : 
+     638             : /*//{ getLastValidHdg() */
+     639           0 : double HdgGeneric::getLastValidHdg() const {
+     640           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     641             : }
+     642             : /*//}*/
+     643             : 
+     644             : /*//{ callbackReconfigure() */
+     645           0 : void HdgGeneric::callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     646             : 
+     647           0 :   if (!isInitialized()) {
+     648           0 :     return;
+     649             :   }
+     650             : 
+     651           0 :   Q_t Q;
+     652           0 :   Q(POSITION, POSITION) = config.pos;
+     653           0 :   Q(VELOCITY, VELOCITY) = config.vel;
+     654           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     655             : }
+     656             : /*//}*/
+     657             : 
+     658             : /*//{ getNamespacedName() */
+     659           0 : std::string HdgGeneric::getNamespacedName() const {
+     660           0 :   return parent_state_est_name_ + "/" + getName();
+     661             : }
+     662             : /*//}*/
+     663             : 
+     664             : /*//{ getPrintName() */
+     665           0 : std::string HdgGeneric::getPrintName() const {
+     666           0 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     667             : }
+     668             : /*//}*/
+     669             : 
+     670             : };  // 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..359f76a76f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html @@ -0,0 +1,188 @@ + + + + + + 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 + + +
+ 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..0b09042fa36a2f106e7a304e58266487570d373d GIT binary patch literal 2195 zcmV;E2yFL>P)2nt zU)m3A)4%)2dJ%latVl(VUKl_c!Xrkg5)Z5AOhSBV3s~CEh*2If;fdZAW7H=FgE>d8 zdDx##b~byC)P}%E7~VGq2q6P*3df~p#}*YF{lSo}$?ow9VLJaPS%zOau9_v4cnpf6 zJR($IpQ5(*zddHcG>0=6Sf)mJLN=zyEFcjEt+N; z)Z_Q-GiUwH$=#?PSq_<{3HRTm=L<$hrP;o_wFveKk5`5i3ZEQP@}{|wKnNEjfF*jf zG&DzC@+vq&m)7bW!i)uB_IZJjz(g{A$Z-K-V5v=Mt2LmP zBZN0ibZNgQSo_tEE7o_eCfwxi>N*py#;FRDf$QD2AjI4dcS4g;(T~xA(DRfnBoT(+ zSaI9@T6HvaEq5u7&ujCZBB7V^$C}AXXrC5lrO5jE5+32PpPNlF()ub*4-_2dtYr#A z$cOi-+-BOex+Sc&DH6Icqp)_nv`XkuVf>`6#a$u<2U)s@pMwXVxeJQH(}%*K-(68J ztd@4N!gu}j+$E$l$CegXCYh9Knfqc7Jlgz`mz+qQzU@K476&9ZZRqb7D8w0EI3TU% zsR`F3NN1)MDZ`l&%fxI|B`jr=6*W{hgMmvZ9&Pt}+~WFv;` zUL#06Y6iz)20NLurydxOftenfpO2mw6o0I60tz|sn7aJn*0Q!nHjoP}0}l|l<9&sB zuY&E{w6yfUb7G&;q2uEV6+MODc(gx!$dw7PL+)k3O{rOtm>yEk3Pt9Dly0ywk zbGYvzgY<^;lud%`+xx~qx#mWQUWxe)h2tRwIBYgv82xxNWmJ9gYXrKNITtYFvs_^< z_BrYbw+l1AUq?g~x$#a|9Ekah_esF@Cq8M$bAdY*r`K?iBXyJJoX^)nJ0V)OVNdYG zTf^Q3mu$++NP=(KSLm>NO<%zu*VVhgS(t2PF7@Nevo36hE+$DC;rl2+F3%|Wz;)_L zQrlRz>J$rZJMPJWspV1C;@!e*y3Q~t5m8*dwB9Jdq2hkaL|H6J3is+9;Ssu0eMJDs z&wVVUX4#Zh_L^R}%on?kTmfd8w!5`+>P_ZRk$(wiX_AU?`s7|`n)=g(EKk^uPxZ+! z-TpPN9*hnj>57{|XZ9i`0E&bu&9Cu`B6FKv!?ygD{*ir>R zix)N^Y@_fx9|)kZ^+l7WcJ7P&Adevh&`?=4ayojN>Qq=VCx6u~?zE&6v?rPjdYHlx z2A^uiy>%=Qo}E7$lJ)!Cr~_Q}e}?SU-QBb4&y?nKGyq`2y#1NCKlAoyBH`Pg zdHXXBkL&%e@b+go!nZ%uSNPkXdHXZ%j1K|sFZ5@mAo=!Zy6}DbGX==y8Bgoa+qh;=j>~X?Sc{ zWdK)?@6UYv`k#e7)ENr4pTL&zJ^0zp@5c!+y%>kOEabTKdrjzJVx~3Gtwd#Wwfz1S-_@r&Tm9EXJCZT+8%;OPQ$k) z9~1t_CfJ{WnKTT1L_KaFw60j3(P^(Y2!yWlKs6yen!1K!0d2PL=z)4e;jCcEY;nT1 zAw;)f*fIp*T^`ZU{Qqn=J)!rZ-D+4Ne2?yh>57KW3`ml4NY0;l7%0BeK$~~1G@f3A zlq${sldJnPiM)C31S~Z?L>{a9Jj)~b|F2o55<1mfN+Z!%FjbP5V&YPRbOz#avk()c z*0c%vkHb8!y<~?H4qHaOdBuXOL1PDx4UZ4NBOrAnZEP3Rw{aL-Gjcz{)~ejB1!GM$ zeGYF-V^0Z{8~%m(NO^R{g#iJN)J~g~o`6TyIJ^+_N476@Bg}6|&C43?7XuLMh5qZX z)XX3Aga@QP?uA9rmAtTQ3=c^Sj}DJ|q+lQOza5X`@6gx}Jsgh%?>P74d_v`59|#dH zal+qlE+#rMPj*!Aitm-0UK92RhfO$`o1O6{8~iQ)18<*~$|oFQ#+po+Ha=y>*M#Sp z5L(*AdYIZ9{vi{pR&4im{{EnRR;a?;{TaZ~j5H3Hh5EOepGJjj5+>;(boh8)MhP=y z+8SISdr*j literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html new file mode 100644 index 0000000000..189fa0e9ff --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8813167.2 %
Date:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::HdgPassthrough::start()71
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const78
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const233
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)48653
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const48653
mrs_uav_state_estimators::HdgPassthrough::getStates() const48653
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)49931
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)99110
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)102301
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const177110
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)201395
+
+
+ + + +
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..3647b7247e --- /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:8813167.2 %
Date:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)48653
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)49931
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)102301
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>)99110
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()71
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)201395
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]() const233
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]() const78
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const48653
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const177110
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const48653
+
+
+ + + +
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..1bc509a0de --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html @@ -0,0 +1,387 @@ + + + + + + + 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:8813167.2 %
Date:2023-12-06 07:59:45Functions: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          39 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          39 :   ch_ = ch;
+      17          39 :   ph_ = ph;
+      18             : 
+      19          39 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21          39 :   hdg_state_      = states_t::Zero();
+      22          39 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24             :   // | --------------- param loader initialization --------------- |
+      25             : 
+      26          39 :   if (is_core_plugin_) {
+      27             : 
+      28          39 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      29          39 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          39 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      33             : 
+      34             :   // | --------------------- load parameters -------------------- |
+      35          39 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      36             : 
+      37          39 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      38          39 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      39             : 
+      40          39 :   if (!ph->param_loader->loadedSuccessfully()) {
+      41           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      42           0 :     ros::shutdown();
+      43             :   }
+      44             : 
+      45             :   // | ------------------ timers initialization ----------------- |
+      46          39 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      47          39 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      48             : 
+      49             :   // | --------------- subscribers initialization --------------- |
+      50             :   // subscriber to odometry
+      51          78 :   mrs_lib::SubscribeHandlerOptions shopts;
+      52          39 :   shopts.nh                 = nh;
+      53          39 :   shopts.node_name          = getPrintName();
+      54          39 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      55          39 :   shopts.threadsafe         = true;
+      56          39 :   shopts.autostart          = true;
+      57          39 :   shopts.queue_size         = 10;
+      58          39 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      59             : 
+      60          78 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      61          39 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      62          78 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      63          39 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      64             : 
+      65             :   // | ---------------- publishers initialization --------------- |
+      66          39 :   if (ch_->debug_topics.output) {
+      67          39 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      68             :   }
+      69          39 :   if (ch_->debug_topics.diag) {
+      70           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+      71             :   }
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          39 :   if (changeState(INITIALIZED_STATE)) {
+      76          39 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+      77             :   } else {
+      78           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      79             :   }
+      80          39 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84          71 : bool HdgPassthrough::start(void) {
+      85             : 
+      86          71 :   if (isInState(READY_STATE)) {
+      87          39 :     timer_update_.start();
+      88          39 :     changeState(STARTED_STATE);
+      89          39 :     return true;
+      90             : 
+      91             :   } else {
+      92          32 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      93          32 :     return false;
+      94             :   }
+      95             : }
+      96             : /*//}*/
+      97             : 
+      98             : /*//{ pause() */
+      99           0 : bool HdgPassthrough::pause(void) {
+     100             : 
+     101           0 :   if (isInState(RUNNING_STATE)) {
+     102           0 :     changeState(STOPPED_STATE);
+     103           0 :     return true;
+     104             : 
+     105             :   } else {
+     106           0 :     return false;
+     107             :   }
+     108             : }
+     109             : /*//}*/
+     110             : 
+     111             : /*//{ reset() */
+     112           0 : bool HdgPassthrough::reset(void) {
+     113             : 
+     114           0 :   if (!isInitialized()) {
+     115           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     116           0 :     return false;
+     117             :   }
+     118             : 
+     119           0 :   changeState(STOPPED_STATE);
+     120             : 
+     121           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     122             : 
+     123           0 :   return true;
+     124             : }
+     125             : /*//}*/
+     126             : 
+     127             : /*//{ callbackOrientation() */
+     128      102301 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     129             : 
+     130      102301 :   if (!isInitialized()) {
+     131          16 :     return;
+     132             :   }
+     133             : 
+     134             :   double hdg;
+     135             :   try {
+     136      102285 :     hdg = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     137             :   }
+     138           0 :   catch (...) {
+     139           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     140             :   }
+     141             : 
+     142      102285 :   setState(hdg, POSITION);
+     143             : 
+     144      102285 :   if (!isError()) {
+     145      102285 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     146             :   }
+     147             : }
+     148             : /*//}*/
+     149             : 
+     150             : /*//{ callbackAngularVelocity() */
+     151       99110 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     152             : 
+     153       99110 :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     154           0 :     return;
+     155             :   }
+     156             : 
+     157             :   double hdg_rate;
+     158             :   try {
+     159       99110 :     hdg_rate = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+     160             :   }
+     161           0 :   catch (...) {
+     162           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     163             :   }
+     164             : 
+     165       99110 :   setState(hdg_rate, VELOCITY);
+     166             : }
+     167             : /*//}*/
+     168             : 
+     169             : /* timerUpdate() //{*/
+     170       48653 : void HdgPassthrough::timerUpdate(const ros::TimerEvent &event) {
+     171             : 
+     172             : 
+     173       48653 :   if (!isInitialized()) {
+     174           0 :     return;
+     175             :   }
+     176             : 
+     177       48653 :   publishOutput();
+     178       48653 :   publishDiagnostics();
+     179             : }
+     180             : /*//}*/
+     181             : 
+     182             : /*//{ timerCheckHealth() */
+     183       49931 : void HdgPassthrough::timerCheckHealth(const ros::TimerEvent &event) {
+     184             : 
+     185       49931 :   if (!isInitialized()) {
+     186           3 :     return;
+     187             :   }
+     188             : 
+     189       49928 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     190             : 
+     191          39 :     changeState(READY_STATE);
+     192          39 :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     193             :   }
+     194             : 
+     195       49928 :   if (isInState(STARTED_STATE)) {
+     196             : 
+     197          39 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     198          39 :     changeState(RUNNING_STATE);
+     199             :   }
+     200             : 
+     201       49928 :   if (sh_orientation_.hasMsg()) {
+     202       49410 :     is_orient_ready_ = true;
+     203             :   } else {
+     204         518 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     205             :   }
+     206             : 
+     207       49928 :   if (sh_ang_vel_.hasMsg()) {
+     208       48748 :     is_ang_vel_ready_ = true;
+     209             :   } else {
+     210        1180 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity yet", getPrintName().c_str());
+     211             :   }
+     212             : }
+     213             : /*//}*/
+     214             : 
+     215             : /*//{ getState() */
+     216           0 : double HdgPassthrough::getState(const int &state_id_in, const int &axis_in) const {
+     217           0 :   return getState(stateIdToIndex(state_id_in, 0));
+     218             : }
+     219             : 
+     220      177110 : double HdgPassthrough::getState(const int &state_idx_in) const {
+     221      177110 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     222             : }
+     223             : /*//}*/
+     224             : 
+     225             : /*//{ setState() */
+     226           0 : void HdgPassthrough::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     227           0 :   setState(state_in, stateIdToIndex(state_id_in, 0));
+     228           0 : }
+     229             : 
+     230      201395 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     231             : 
+     232      201395 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     233      201394 :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     234      201395 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     235             : 
+     236      201395 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     237      201391 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     238      201395 : }
+     239             : /*//}*/
+     240             : 
+     241             : /*//{ getStates() */
+     242       48653 : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     243       48653 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_);
+     244             : }
+     245             : /*//}*/
+     246             : 
+     247             : /*//{ setStates() */
+     248           0 : void HdgPassthrough::setStates(const states_t &states_in) {
+     249           0 :   mrs_lib::set_mutexed(mtx_hdg_state_, states_in, hdg_state_);
+     250           0 : }
+     251             : /*//}*/
+     252             : 
+     253             : /*//{ getCovariance() */
+     254           0 : double HdgPassthrough::getCovariance(const int &state_id_in, const int &axis_in) const {
+     255           0 :   return getCovariance(stateIdToIndex(state_id_in, 0));
+     256             : }
+     257             : 
+     258           0 : double HdgPassthrough::getCovariance(const int &state_idx_in) const {
+     259           0 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_(state_idx_in, state_idx_in));
+     260             : }
+     261             : /*//}*/
+     262             : 
+     263             : /*//{ getCovarianceMatrix() */
+     264       48653 : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     265       48653 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_);
+     266             : }
+     267             : /*//}*/
+     268             : 
+     269             : /*//{ setCovarianceMatrix() */
+     270           0 : void HdgPassthrough::setCovarianceMatrix(const covariance_t &cov_in) {
+     271           0 :   mrs_lib::set_mutexed(mtx_hdg_covariance_, cov_in, hdg_covariance_);
+     272           0 : }
+     273             : /*//}*/
+     274             : 
+     275             : /*//{ getInnovation() */
+     276           0 : double HdgPassthrough::getInnovation(const int &state_idx) const {
+     277           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_(state_idx));
+     278             : }
+     279             : 
+     280           0 : double HdgPassthrough::getInnovation(const int &state_id_in, const int &axis_in) const {
+     281           0 :   return getInnovation(stateIdToIndex(state_id_in, 0));
+     282             : }
+     283             : /*//}*/
+     284             : 
+     285             : /*//{ getLastValidHdg() */
+     286           0 : double HdgPassthrough::getLastValidHdg() const {
+     287           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     288             : }
+     289             : /*//}*/
+     290             : 
+     291             : /*//{ getNamespacedName() */
+     292          78 : std::string HdgPassthrough::getNamespacedName() const {
+     293         156 :   return parent_state_est_name_ + "/" + getName();
+     294             : }
+     295             : /*//}*/
+     296             : 
+     297             : /*//{ getPrintName() */
+     298         233 : std::string HdgPassthrough::getPrintName() const {
+     299         466 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     300             : }
+     301             : /*//}*/
+     302             : 
+     303             : };  // 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..a0f4905b86 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html @@ -0,0 +1,96 @@ + + + + + + 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 + + +
+ 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..d11291d3d4e80dd6164377556c01c1c9fb3b1924 GIT binary patch literal 1189 zcmV;W1X}xvP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpKAk1!HSC9?D3U&kBf0YyYg-=58>8p2_>sh5~AYYQu>HU7s{{KBsSHSyq z(3{_vP&+8@V2N@tNbwq0i#5*Y7u0~umpFML&RtDwt#kw67)KvXix8$o18BGuQ9ktn zO2v0~TTU`caleBo8Fp&RlIBq5Q|~$qp71(eb9!>`qeihLg({wF=^B?s_u$aL0HVMk zoOtA1H^JH-;DJ+`In*&{YS|Svvr~sz6Q{!#-X%&S>g3iiICV2pRF*WY>*nEOU!TqF zf8p6*?EpetS(sMCBSb_wAQjF@c{8O#D=KKQAEbZ;4jn|P!1Dl5FEXytNBh}r$0M#_ zHAw9DFnOh@lS6ZO+p+4DsTEfYR8WV6x`L z<_NVM{ly0R4-WUEaulk1nKbG01)^LrzCh|CmRU0;s|~ui^#q8`EO(*(iSiklYgxpcqM@7;w=A79A(r-1W>)8< zT_ehay@~FfEPgnN6GN!41?h~C>tH>s=`3{f3Xtu3#9xzB2ppDh*J}8byQskwZe)kl=OtL~JpUXFjzU4yyMMTNMEyTKJ+v1!I5He5lW_h`KXg^v`(Tih3Y zCU3AYYeaFBRr^#G{N4psx0lj;D{jGGS>L8^^j$*nxeKbTbxYYvy-;>I|e?Qk; zqw);b3isKvopGc)^G&WJCv1C2-Yu-f15Pk1jJTV-xn}$jtr6%$h3mLh+ME0c*AGZE z*&u3RD;pK*9MHsjW3!YR(|uCzT;gh247!T;5FJ%z89_o@OP${8{LQc#RM6!YG`g>wmsDsZ9y8^$)RHJii%;1;waXCLs&PaG69(;}$cDZI1czD<+>AHgKG{6kn zA_%EIxG8RJ?E>>BTl6t~(VubLJwNFiyBB-d&5NnG=6H&Pcrl2>YF>`6c3 zEJMm8VoxU%Z*r~bqB7??{;Iha!B@HVeOLVJ{nive>hpUUg(oE<00000NkvXXu0mjf D9-b<6 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html new file mode 100644 index 0000000000..39bb785f1f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2023-12-06 07:59:45Functions:125422.2 %
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 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
<unnamed>67.2 %88 / 13152.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..557d84559d --- /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:8846618.9 %
Date:2023-12-06 07:59:45Functions:125422.2 %
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 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
<unnamed>67.2 %88 / 13152.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..26c2bad0e7 --- /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:8846618.9 %
Date:2023-12-06 07:59:45Functions:125422.2 %
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 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
<unnamed>67.2 %88 / 13152.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..1902967a2f --- /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:8846618.9 %
Date:2023-12-06 07:59:45Functions:125422.2 %
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 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.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..c0ed541d8e --- /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:8846618.9 %
Date:2023-12-06 07:59:45Functions:125422.2 %
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 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.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..3743f4103f --- /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:8846618.9 %
Date:2023-12-06 07:59:45Functions:125422.2 %
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 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.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..7fc9a0c76e --- /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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.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.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
<unnamed>58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
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..78d251b3ef --- /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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.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.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
<unnamed>58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
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..4b065bf495 --- /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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.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.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
<unnamed>58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
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..bd143cd6bf --- /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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.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.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
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..4c545d7448 --- /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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.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.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
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..a8d1bbf0ac --- /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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.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.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
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..e20ddf5cf3 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + 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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.3 %
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::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::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&)39
mrs_uav_state_estimators::LatGeneric::isConverged()39
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)39
mrs_uav_state_estimators::LatGeneric::start()110
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)170
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)170
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const236
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const353
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) const2082
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)48466
mrs_uav_state_estimators::LatGeneric::setDt(double const&)48466
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const48466
mrs_uav_state_estimators::LatGeneric::getStates() const48466
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)49810
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const96044
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const96044
mrs_uav_state_estimators::LatGeneric::generateA()96971
mrs_uav_state_estimators::LatGeneric::generateB()96971
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&)102691
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)102691
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) const102691
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const192088
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const192088
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const486444
mrs_uav_state_estimators::LatGeneric::getState(int const&) const486450
+
+
+ + + +
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..ef7dbbee96 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + 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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.3 %
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&)39
mrs_uav_state_estimators::LatGeneric::isConverged()39
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)49810
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&)102691
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)102691
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)48466
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)39
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setDt(double const&)48466
mrs_uav_state_estimators::LatGeneric::start()110
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)170
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)170
mrs_uav_state_estimators::LatGeneric::generateA()96971
mrs_uav_state_estimators::LatGeneric::generateB()96971
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const353
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const192088
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const192088
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const96044
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const96044
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const236
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const48466
mrs_uav_state_estimators::LatGeneric::getState(int const&) const486450
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const486444
mrs_uav_state_estimators::LatGeneric::getStates() const48466
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) const102691
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) const2082
+
+
+ + + +
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..fbc3c74781 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html @@ -0,0 +1,838 @@ + + + + + + + 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:23640558.3 %
Date:2023-12-06 07:59:45Functions:253083.3 %
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          39 : void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      14             : 
+      15          39 :   ch_ = ch;
+      16          39 :   ph_ = ph;
+      17             : 
+      18          39 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      19             : 
+      20             :   // clang-format off
+      21          39 :   dt_ = 0.01;
+      22          39 :   input_coeff_ = 10;
+      23          39 :   default_input_coeff_ = 10;
+      24             : 
+      25          39 :   generateA();
+      26          39 :   generateB();
+      27             : 
+      28          39 :   H_ <<
+      29          39 :     1, 0, 0, 0, 0, 0,
+      30          39 :     0, 1, 0, 0, 0, 0;
+      31             : 
+      32             :   // clang-format on
+      33             : 
+      34             :   // | --------------- initialize parameter loader -------------- |
+      35             : 
+      36          39 :   if (is_core_plugin_) {
+      37             : 
+      38          39 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      39          39 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      40             :   }
+      41             : 
+      42          39 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      43             : 
+      44             :   // | --------------------- load parameters -------------------- |
+      45          39 :   ph->param_loader->loadParam("hdg_source_topic", hdg_source_topic_);
+      46          39 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      47          39 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      48          39 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      49          39 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      50          39 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      51          39 :   if (is_repredictor_enabled_) {
+      52           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      53             :   }
+      54             : 
+      55             :   // | --------------- corrections initialization --------------- |
+      56          39 :   ph->param_loader->loadParam("corrections", correction_names_);
+      57             : 
+      58          80 :   for (auto corr_name : correction_names_) {
+      59          41 :     corrections_.push_back(std::make_shared<Correction<lat_generic::n_measurements>>(
+      60        2164 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::LATERAL, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      61      102691 :         [this](const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      62      102691 :           return this->doCorrection(meas, R, state);
+      63             :         }));
+      64             :   }
+      65             : 
+      66             :   // | ------- check if all parameters loaded successfully ------ |
+      67          39 :   if (!ph->param_loader->loadedSuccessfully()) {
+      68           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      69           0 :     ros::shutdown();
+      70             :   }
+      71             : 
+      72          39 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      73             : 
+      74             :   // | ----------- initialize process noise covariance ---------- |
+      75          39 :   Q_ = Q_t::Zero();
+      76             :   double tmp_noise;
+      77          39 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      78          39 :   Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X)) = tmp_noise;
+      79          39 :   Q_(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y)) = tmp_noise;
+      80          39 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      81          39 :   Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X)) = tmp_noise;
+      82          39 :   Q_(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y)) = tmp_noise;
+      83          39 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      84          39 :   Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = tmp_noise;
+      85          39 :   Q_(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = tmp_noise;
+      86             : 
+      87             :   // | ------------- initialize dynamic reconfigure ------------- |
+      88             :   drmgr_ =
+      89          39 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&LatGeneric::callbackReconfigure, this, _1, _2));
+      90          39 :   drmgr_->config.pos = Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X));
+      91          39 :   drmgr_->config.vel = Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X));
+      92          39 :   drmgr_->config.acc = Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X));
+      93          39 :   drmgr_->update_config(drmgr_->config);
+      94             : 
+      95             :   // | --------------- Kalman filter intialization -------------- |
+      96          39 :   const x_t        x0 = x_t::Zero();
+      97          39 :   const P_t        P0 = 1e3 * P_t::Identity();
+      98          39 :   const statecov_t sc0({x0, P0});
+      99          39 :   sc_ = sc0;
+     100             : 
+     101          39 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     102          39 :   if (is_repredictor_enabled_) {
+     103             : 
+     104           0 :     for (int i = 0; i < lat_generic::n_states; i++) {
+     105           0 :       H_t H                = H_t::Zero();
+     106           0 :       H(AXIS_X, i * 2)     = 1;
+     107           0 :       H(AXIS_Y, i * 2 + 1) = 1;
+     108           0 :       models_.push_back(std::make_shared<lkf_t>(A_, B_, H));
+     109             :     }
+     110             : 
+     111           0 :     const u_t       u0 = u_t::Zero();
+     112           0 :     const ros::Time t0 = ros::Time::now();
+     113           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_);
+     114             : 
+     115           0 :     setDt(1.0 / ch_->desired_uav_state_rate);
+     116             :   }
+     117             : 
+     118             :   // | ------------------ timers initialization ----------------- |
+     119          39 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerUpdate, this);  // not running after init
+     120             :   /* timer_check_health_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerCheckHealth, this); */
+     121             : 
+     122             :   // | --------------- subscribers initialization --------------- |
+     123             :   // subscriber to odometry
+     124          78 :   mrs_lib::SubscribeHandlerOptions shopts;
+     125          39 :   shopts.nh                 = nh;
+     126          39 :   shopts.node_name          = getPrintName();
+     127          39 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     128          39 :   shopts.threadsafe         = true;
+     129          39 :   shopts.autostart          = true;
+     130          39 :   shopts.queue_size         = 10;
+     131          39 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     132             : 
+     133          39 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     134             :   sh_hdg_state_ =
+     135          39 :       mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput>(shopts, hdg_source_topic_);  // for transformation of desired accelerations from body to global frame
+     136             : 
+     137             :   // | ---------------- publishers initialization --------------- |
+     138          39 :   if (ch_->debug_topics.input) {
+     139          39 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     140             :   }
+     141          39 :   if (ch_->debug_topics.output) {
+     142          39 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     143             :   }
+     144          39 :   if (ch_->debug_topics.diag) {
+     145           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+     146             :   }
+     147             : 
+     148             :   // | ------------------ finish initialization ----------------- |
+     149             : 
+     150          39 :   if (changeState(INITIALIZED_STATE)) {
+     151          39 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+     152             :   } else {
+     153           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+     154             :   }
+     155          39 : }
+     156             : /*//}*/
+     157             : 
+     158             : /*//{ start() */
+     159         110 : bool LatGeneric::start(void) {
+     160             : 
+     161         110 :   if (isInState(READY_STATE)) {
+     162             :     /* timer_update_.start(); */
+     163          39 :     changeState(STARTED_STATE);
+     164          39 :     return true;
+     165             : 
+     166             :   } else {
+     167          71 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     168          71 :     return false;
+     169             :   }
+     170             : }
+     171             : /*//}*/
+     172             : 
+     173             : /*//{ pause() */
+     174           0 : bool LatGeneric::pause(void) {
+     175             : 
+     176           0 :   if (isInState(RUNNING_STATE)) {
+     177           0 :     changeState(STOPPED_STATE);
+     178           0 :     return true;
+     179             : 
+     180             :   } else {
+     181           0 :     return false;
+     182             :   }
+     183             : }
+     184             : /*//}*/
+     185             : 
+     186             : /*//{ reset() */
+     187           0 : bool LatGeneric::reset(void) {
+     188             : 
+     189           0 :   if (!isInitialized()) {
+     190           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     191           0 :     return false;
+     192             :   }
+     193             : 
+     194           0 :   changeState(STOPPED_STATE);
+     195             : 
+     196             :   // Initialize LKF state and covariance
+     197           0 :   const x_t        x0 = x_t::Zero();
+     198           0 :   const P_t        P0 = 1e6 * P_t::Identity();
+     199           0 :   const statecov_t sc0({x0, P0});
+     200           0 :   sc_ = sc0;
+     201             : 
+     202             :   // Instantiate the LKF itself
+     203           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     204           0 :   if (is_repredictor_enabled_) {
+     205             : 
+     206           0 :     const u_t       u0 = u_t::Zero();
+     207           0 :     const ros::Time t0 = ros::Time(0);
+     208           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_);
+     209             :   }
+     210             : 
+     211           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     212             : 
+     213           0 :   return true;
+     214             : }
+     215             : /*//}*/
+     216             : 
+     217             : /* timerUpdate() //{*/
+     218       49810 : void LatGeneric::timerUpdate(const ros::TimerEvent &event) {
+     219             : 
+     220       49810 :   if (!isInitialized()) {
+     221        1344 :     return;
+     222             :   }
+     223             : 
+     224       49805 :   switch (getCurrentSmState()) {
+     225             : 
+     226           0 :     case UNINITIALIZED_STATE: {
+     227           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     228           0 :       break;
+     229             :     }
+     230             : 
+     231          14 :     case READY_STATE: {
+     232          14 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     233          14 :       break;
+     234             :     }
+     235             : 
+     236        1173 :     case INITIALIZED_STATE: {
+     237             :       // initialize the estimator with current corrections
+     238        1258 :       for (auto correction : corrections_) {
+     239        1219 :         auto res = correction->getProcessedCorrection();
+     240        1219 :         if (res) {
+     241          85 :           auto measurement_stamped = res.value(); 
+     242          85 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     243          85 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     244          85 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+     245             :                             measurement_stamped.value(AXIS_Y));
+     246             :         } else {
+     247        1134 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getNamespacedName().c_str());
+     248        1134 :           return;
+     249             :         }
+     250             :       }
+     251          39 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     252          39 :       changeState(READY_STATE);
+     253          39 :       break;
+     254             :     }
+     255             : 
+     256          39 :     case STARTED_STATE: {
+     257          39 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     258          39 :       if (isConverged()) {
+     259          39 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     260          39 :         changeState(RUNNING_STATE);
+     261             :       }
+     262          39 :       break;
+     263             :     }
+     264             : 
+     265       48579 :     case RUNNING_STATE: {
+     266       99170 :       for (auto correction : corrections_) {
+     267       50591 :         if (!correction->isHealthy()) {
+     268           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     269           0 :           changeState(ERROR_STATE);
+     270             :         }
+     271             :       }
+     272       48579 :       break;
+     273             :     }
+     274             : 
+     275           0 :     case STOPPED_STATE: {
+     276           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     277           0 :       break;
+     278             :     }
+     279             : 
+     280           0 :     case ERROR_STATE: {
+     281           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     282             : 
+     283           0 :       ros::Time t_now = ros::Time::now();
+     284           0 :       if (is_error_state_first_time_) {
+     285           0 :         prev_time_in_error_state_  = t_now;
+     286           0 :         is_error_state_first_time_ = false;
+     287           0 :         error_state_duration_      = ros::Duration(0.0);
+     288             :       }
+     289           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     290             : 
+     291             : 
+     292             :       // check if all corrections are healthy now
+     293           0 :       bool all_corrections_healthy = true;
+     294           0 :       for (auto correction : corrections_) {
+     295           0 :         if (!correction->isHealthy()) {
+     296           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     297           0 :           all_corrections_healthy = false;
+     298             :         }
+     299             :       }
+     300             : 
+     301           0 :       if (all_corrections_healthy && innovation_ok_) {
+     302             :         // initialize the estimator again if corrections become healthy
+     303           0 :         if (error_state_duration_.toSec() > 5.0) {
+     304           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     305           0 :           changeState(INITIALIZED_STATE);
+     306           0 :           is_error_state_first_time_ = true;
+     307           0 :         }
+     308             :       } else {
+     309           0 :         is_error_state_first_time_ = true;
+     310             :       }
+     311             : 
+     312           0 :       prev_time_in_error_state_ = t_now;
+     313             : 
+     314           0 :       break;
+     315             :     }
+     316             :   }
+     317             : 
+     318       48671 :   if (sh_control_input_.newMsg()) {
+     319       25804 :     is_input_ready_ = true;
+     320             :   }
+     321             : 
+     322             :   // check age of input
+     323       48671 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     324           3 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f s), using zero input instead", getPrintName().c_str(),
+     325             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     326           3 :     is_input_ready_ = false;
+     327             :   }
+     328             : 
+     329       48671 :   if (fun_get_hdg_()) {
+     330       48603 :     is_hdg_state_ready_ = true;
+     331             :   }
+     332             : 
+     333       48671 :   if (!isRunning() && !isStarted()) {
+     334          49 :     return;
+     335             :   }
+     336             : 
+     337       48622 :   if (first_iter_) {
+     338          39 :     first_iter_ = false;
+     339          39 :     return;
+     340             :   }
+     341             : 
+     342             :   // obtain dt for state prediction
+     343       48583 :   double dt = (event.current_real - event.last_real).toSec();
+     344       48583 :   if (dt <= 0.0) {  // sometimes the timer ticks twice simultaneously in simulation - we ignore the second tick
+     345         117 :     return;
+     346             :   }
+     347             : 
+     348       48466 :   if (!is_repredictor_enabled_) {  // repredictor requires constant dt TODO: how to handle repredictor + variable rate?
+     349       48466 :     setDt(dt);
+     350             :   }
+     351             : 
+     352             :   // obtain unbiased desired control acceleration in the estimator frame that will be used as input to the estimator
+     353       48466 :   u_t       u;
+     354       48466 :   ros::Time input_stamp;
+     355       48466 :   if (is_input_ready_ && is_hdg_state_ready_) {
+     356       31850 :     auto res = fun_get_hdg_();
+     357       31850 :     if (!res) {
+     358           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not obtain heading", getPrintName().c_str());
+     359           0 :       return;
+     360             :     }
+     361             : 
+     362       31850 :     const tf2::Vector3 des_acc_global = getAccGlobal(sh_control_input_.getMsg(), res.value());
+     363       31850 :     input_stamp                       = sh_control_input_.getMsg()->header.stamp;
+     364       31850 :     setInputCoeff(default_input_coeff_);
+     365       31850 :     u(0) = des_acc_global.getX();
+     366       31850 :     u(1) = des_acc_global.getY();
+     367             : 
+     368             :   } else {  // this is ok before the controller starts controlling but bad during actual flight (causes delayed estimated acceleration and velocity)
+     369       16616 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     370       16616 :     input_stamp = ros::Time::now();
+     371       16616 :     setInputCoeff(0);
+     372       16616 :     u = u_t::Zero();
+     373             :   }
+     374             : 
+     375             :   // go through available corrections and apply them
+     376             :   /* for (auto correction : corrections_) { */
+     377             :   /*   auto res = correction->getProcessedCorrection(); */
+     378             :   /*   if (res) { */
+     379             :   /*     auto measurement_stamped = res.value(); */
+     380             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     381             :   /*   } */
+     382             :   /* } */
+     383             : 
+     384             :   // get current state, covariance, and process noise
+     385       48466 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     386       48466 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     387             : 
+     388             :   // prediction step
+     389             :   try {
+     390             :     // Apply the prediction step
+     391       96932 :     std::scoped_lock lock(mutex_lkf_);
+     392       48466 :     if (is_repredictor_enabled_) {
+     393           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, lkf_);
+     394           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     395             :     } else {
+     396       48466 :       sc = lkf_->predict(sc, u, Q, dt_);
+     397             :     }
+     398             :   }
+     399           0 :   catch (const std::exception &e) {
+     400           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     401           0 :     changeState(ERROR_STATE);
+     402             :   }
+     403             : 
+     404             :   // update the state and covariance variable that is queried by the estimation manager
+     405       48466 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     406             : 
+     407             :   // publishing
+     408       48466 :   publishInput(u, input_stamp);
+     409       48466 :   publishOutput();
+     410       48466 :   publishDiagnostics();
+     411             : }
+     412             : /*//}*/
+     413             : 
+     414             : /*//{ timerCheckHealth() */
+     415           0 : void LatGeneric::timerCheckHealth(const ros::TimerEvent &event) {
+     416             : 
+     417           0 :   if (!isInitialized()) {
+     418           0 :     return;
+     419             :   }
+     420             : 
+     421           0 :   switch (getCurrentSmState()) {
+     422             : 
+     423           0 :     case UNINITIALIZED_STATE: {
+     424           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     425           0 :       break;
+     426             :     }
+     427             : 
+     428           0 :     case READY_STATE: {
+     429           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     430           0 :       break;
+     431             :     }
+     432             : 
+     433           0 :     case INITIALIZED_STATE: {
+     434             :       // initialize the estimator with current corrections
+     435           0 :       for (auto correction : corrections_) {
+     436           0 :         auto res = correction->getProcessedCorrection();
+     437           0 :         if (res) {
+     438           0 :           auto measurement_stamped = res.value();
+     439           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     440           0 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     441           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+     442             :                             measurement_stamped.value(AXIS_Y));
+     443             :         } else {
+     444           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getPrintName().c_str());
+     445           0 :           return;
+     446             :         }
+     447             :       }
+     448           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     449           0 :       changeState(READY_STATE);
+     450           0 :       break;
+     451             :     }
+     452             : 
+     453           0 :     case STARTED_STATE: {
+     454           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     455           0 :       if (isConverged()) {
+     456           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     457           0 :         changeState(RUNNING_STATE);
+     458             :       }
+     459           0 :       break;
+     460             :     }
+     461             : 
+     462           0 :     case RUNNING_STATE: {
+     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 :           changeState(ERROR_STATE);
+     467             :         }
+     468             :       }
+     469           0 :       break;
+     470             :     }
+     471             : 
+     472           0 :     case STOPPED_STATE: {
+     473           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     474           0 :       break;
+     475             :     }
+     476             : 
+     477           0 :     case ERROR_STATE: {
+     478           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     479           0 :       bool all_corrections_healthy = true;
+     480           0 :       for (auto correction : corrections_) {
+     481           0 :         if (!correction->isHealthy()) {
+     482           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     483           0 :           correction->resetProcessors();
+     484           0 :           all_corrections_healthy = false;
+     485             :         }
+     486             :       }
+     487             :       // initialize the estimator again if corrections become healthy
+     488           0 :       if (all_corrections_healthy) {
+     489           0 :         changeState(INITIALIZED_STATE);
+     490             :       }
+     491           0 :       break;
+     492             :     }
+     493             :   }
+     494             : 
+     495           0 :   if (sh_control_input_.newMsg()) {
+     496           0 :     is_input_ready_ = true;
+     497             :   }
+     498             : 
+     499             :   // check age of input
+     500           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     501           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f s), using zero input instead", getPrintName().c_str(),
+     502             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     503           0 :     is_input_ready_ = false;
+     504             :   }
+     505             : 
+     506           0 :   if (fun_get_hdg_()) {
+     507           0 :     is_hdg_state_ready_ = true;
+     508             :   }
+     509             : }
+     510             : /*//}*/
+     511             : 
+     512             : /*//{ doCorrection() */
+     513      102691 : void LatGeneric::doCorrection(const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     514      102691 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     515      102691 : }
+     516             : /*//}*/
+     517             : 
+     518             : /*//{ doCorrection() */
+     519      102691 : void LatGeneric::doCorrection(const z_t &z, const double R, const StateId_t &state_id, const ros::Time &meas_stamp) {
+     520             : 
+     521      102691 :   if (!isInitialized()) {
+     522         115 :     return;
+     523             :   }
+     524             : 
+     525             :   // we do not want to perform corrections until the estimator is initialized
+     526      102689 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     527         113 :     return;
+     528             :   }
+     529             : 
+     530             :   // for position state check the innovation
+     531      102576 :   if (state_id == POSITION) {
+     532             :     {
+     533       98118 :       std::scoped_lock lock(mtx_innovation_);
+     534             : 
+     535       98118 :       is_mitigating_jump_ = false;
+     536       98118 :       innovation_(0)      = z(0) - getState(POSITION, AXIS_X);
+     537       98118 :       innovation_(1)      = z(1) - getState(POSITION, AXIS_Y);
+     538             : 
+     539       98118 :       if (fabs(innovation_(0)) > pos_innovation_limit_ || fabs(innovation_(1)) > pos_innovation_limit_) {
+     540           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f %.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), innovation_(1),
+     541             :                           pos_innovation_limit_);
+     542           0 :         innovation_ok_ = false;
+     543           0 :         switch (exc_innovation_action_) {
+     544           0 :           case ExcInnoAction_t::ELAND: {
+     545           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", ros::this_node::getName().c_str());
+     546           0 :             changeState(ERROR_STATE);
+     547           0 :             break;
+     548             :           }
+     549           0 :           case ExcInnoAction_t::SWITCH: {
+     550           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", ros::this_node::getName().c_str());
+     551           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?
+     552           0 :             innovation_(1) = 0.0;
+     553           0 :             changeState(ERROR_STATE);
+     554           0 :             break;
+     555             :           }
+     556           0 :           case ExcInnoAction_t::MITIGATE: {
+     557           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", ros::this_node::getName().c_str());
+     558           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?
+     559           0 :             innovation_(1)      = 0.0;
+     560           0 :             is_mitigating_jump_ = true;
+     561           0 :             setState(z(0), POSITION, AXIS_X);
+     562           0 :             setState(z(1), POSITION, AXIS_Y);
+     563           0 :             break;
+     564             :           }
+     565           0 :           case ExcInnoAction_t::NONE: {
+     566           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", ros::this_node::getName().c_str());
+     567           0 :             break;
+     568             :           }
+     569             :         }
+     570             :       }
+     571       98118 :       innovation_ok_ = true;
+     572             :     }
+     573             :   }
+     574             : 
+     575      102576 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     576             :   try {
+     577             :     // Apply the correction step
+     578      205152 :     std::scoped_lock lock(mutex_lkf_);
+     579      102576 :     H_                           = H_t::Zero();
+     580      102576 :     H_(AXIS_X, state_id * 2)     = 1;
+     581      102576 :     H_(AXIS_Y, state_id * 2 + 1) = 1;
+     582      102576 :     lkf_->H                      = H_;
+     583             : 
+     584      102576 :     if (is_repredictor_enabled_) {
+     585             : 
+     586           0 :       lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[state_id]);
+     587             :     } else {
+     588      102576 :       sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     589             :     }
+     590             :   }
+     591           0 :   catch (const std::exception &e) {
+     592             :     // In case of error, alert the user
+     593           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     594             :   }
+     595             : 
+     596      102576 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     597             : }  // namespace mrs_uav_state_estimators
+     598             : /*//}*/
+     599             : 
+     600             : /*//{ isConverged() */
+     601          39 : bool LatGeneric::isConverged() {
+     602             : 
+     603             :   // TODO: check convergence by rate of change of determinant
+     604             : 
+     605          39 :   return true;
+     606             : }
+     607             : /*//}*/
+     608             : 
+     609             : /*//{ getState() */
+     610      486444 : double LatGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     611      486444 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     612             : }
+     613             : 
+     614      486450 : double LatGeneric::getState(const int &state_idx_in) const {
+     615      486450 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     616             : }
+     617             : /*//}*/
+     618             : 
+     619             : /*//{ setState() */
+     620         170 : void LatGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     621         170 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     622         170 : }
+     623             : 
+     624         170 : void LatGeneric::setState(const double &state_in, const int &state_idx_in) {
+     625         170 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     626         170 : }
+     627             : /*//}*/
+     628             : 
+     629             : /*//{ getStates() */
+     630       48466 : LatGeneric::states_t LatGeneric::getStates(void) const {
+     631       96932 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     632             : }
+     633             : /*//}*/
+     634             : 
+     635             : /*//{ setStates() */
+     636           0 : void LatGeneric::setStates(const states_t &states_in) {
+     637           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     638           0 : }
+     639             : /*//}*/
+     640             : 
+     641             : /*//{ getCovariance() */
+     642      192088 : double LatGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     643      192088 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     644             : }
+     645             : 
+     646      192088 : double LatGeneric::getCovariance(const int &state_idx_in) const {
+     647      192088 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     648             : }
+     649             : /*//}*/
+     650             : 
+     651             : /*//{ getCovarianceMatrix() */
+     652       48466 : LatGeneric::covariance_t LatGeneric::getCovarianceMatrix(void) const {
+     653       96932 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     654             : }
+     655             : /*//}*/
+     656             : 
+     657             : /*//{ setCovarianceMatrix() */
+     658           0 : void LatGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     659           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     660           0 : }
+     661             : /*//}*/
+     662             : 
+     663             : /*//{ getInnovation() */
+     664       96044 : double LatGeneric::getInnovation(const int &state_idx) const {
+     665       96044 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     666             : }
+     667             : 
+     668       96044 : double LatGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     669       96044 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     670             : }
+     671             : /*//}*/
+     672             : 
+     673             : /*//{ setDt() */
+     674       48466 : void LatGeneric::setDt(const double &dt) {
+     675       48466 :   dt_ = dt;
+     676       48466 :   generateA();
+     677       48466 :   generateB();
+     678       96932 :   std::scoped_lock lock(mutex_lkf_);
+     679       48466 :   lkf_->A = A_;
+     680       48466 :   lkf_->B = B_;
+     681       48466 : }
+     682             : /*//}*/
+     683             : 
+     684             : /*//{ setInputCoeff() */
+     685       48466 : void LatGeneric::setInputCoeff(const double &input_coeff) {
+     686       48466 :   input_coeff_ = input_coeff;
+     687       48466 :   generateA();
+     688       48466 :   generateB();
+     689       96932 :   std::scoped_lock lock(mutex_lkf_);
+     690       48466 :   lkf_->A = A_;
+     691       48466 :   lkf_->B = B_;
+     692       48466 : }
+     693             : /*//}*/
+     694             : 
+     695             : /*//{ generateA() */
+     696       96971 : void LatGeneric::generateA() {
+     697             : 
+     698             :   // clang-format off
+     699       96971 :     A_ <<
+     700       96971 :       1, 0, dt_, 0, 0.5 * dt_ * dt_, 0,
+     701       96971 :       0, 1, 0, dt_, 0, 0.5 * dt_ * dt_,
+     702       96971 :       0, 0, 1, 0, dt_, 0,
+     703       96971 :       0, 0, 0, 1, 0, dt_,
+     704       96971 :       0, 0, 0, 0, 1-(input_coeff_ * dt_), 0,
+     705       96971 :       0, 0, 0, 0, 0, 1-(input_coeff_ * dt_);
+     706             :   // clang-format on
+     707       96971 : }
+     708             : /*//}*/
+     709             : 
+     710             : /*//{ generateB() */
+     711       96971 : void LatGeneric::generateB() {
+     712             : 
+     713             :   // clang-format off
+     714       96971 :     B_ <<
+     715       96971 :       0, 0,
+     716       96971 :       0, 0,
+     717       96971 :       0, 0,
+     718       96971 :       0, 0,
+     719       96971 :       input_coeff_ * dt_, 0,
+     720       96971 :       0, input_coeff_ * dt_;
+     721             :   // clang-format on
+     722       96971 : }
+     723             : /*//}*/
+     724             : 
+     725             : /*//{ callbackReconfigure() */
+     726          39 : void LatGeneric::callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     727             : 
+     728          39 :   if (!isInitialized()) {
+     729          39 :     return;
+     730             :   }
+     731             : 
+     732           0 :   Q_t Q;
+     733           0 :   Q(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X))         = config.pos;
+     734           0 :   Q(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y))         = config.pos;
+     735           0 :   Q(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X))         = config.vel;
+     736           0 :   Q(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y))         = config.vel;
+     737           0 :   Q(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = config.acc;
+     738           0 :   Q(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = config.acc;
+     739           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     740             : }
+     741             : /*//}*/
+     742             : 
+     743             : /*//{ getNamespacedName() */
+     744         236 : std::string LatGeneric::getNamespacedName() const {
+     745         472 :   return parent_state_est_name_ + "/" + getName();
+     746             : }
+     747             : /*//}*/
+     748             : 
+     749             : /*//{ getPrintName() */
+     750         353 : std::string LatGeneric::getPrintName() const {
+     751         706 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     752             : }
+     753             : /*//}*/
+     754             : };  // 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..557944d153 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html @@ -0,0 +1,209 @@ + + + + + + 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 + + +
+ 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..c3b67fa41f0c650fd83d52174734d752690228e0 GIT binary patch literal 2974 zcmV;P3t{w$P)_0;S;5*sS;5}G*?*N6>jy}HP-&t2xe=DV_f0De-3RF5u(g9?kIKUGj@*F^*|@c>nOH z0@@!PhI8fb__BV89V0NZ!^c33ff_VN8({0zE3I1t*e)55ajC$3_*XN>1I|G8C2q`` zsYs3mkR9E@NAWgumod z^swKWqwcy+*>;EgnS!t}d#$5a^cvX_IsXC}6JX+&0$6iX1oR1`@a*={T|jRoCLnEB z+!sLOK#$hs2R0<%3yPK9H4T?BTIYzwY*xi=z=j)04jzCt2T(m)WK#~Ml=wDomhV1V zULc4SYJ*mdPg*ncXhSKUa)@1h`4MH%(Nx8B=#!d^YWbLc9Gm5qx0iSLXk6-LdRTI^ z%ScWtd(>S(>c-2A8cL?x1a~2jUUU%aJcWue(9@rzaZJ-}jy^COK`fn5M7o%iDRb;w zT&txpD_Likeq)%$jH=Kk>x@mc4nTsj_PzKP zh=dU$hiL1;^gq!)dcn$=hf!`vBM$wIJg~Q1E;W*i&WEX~;gTvT&ciBmG(P%}aFA@--dE|cVDR(*!;-lt zK#M${o;N#I#`RaQ4#Zt9o2A$XMglbOaWO9TJH+VpggK^SVsV%Y9W`LCYC;DMk1;%) zi^N+%*N+WzsS@xBMXFNMYzN6m`1b)*T#>$gVwbOzE-l(J$77hBQufig?u&R^04*6L zlc0qu8pW(-j&BsfH_0n4fcyrX>Dh38U2=qpR|dA04#ozhZ{@K8R8pTt{`?w-+ii~_ zVHo`$yz?Z#_}G|s?PlG_{rEnCv4av2zY{>q?@y%)tb8~d~D?#Su=@kc9DE+hZOz(csuq9wGMZD8FC2 zuJ$Y!3?C!qHO~oqAZ-+|Z?)}d%JGsGGD`C&3_sws#2RH!|H%VdIJATYDI3bAoNnw- z?=7tBT#^TZ>RfsQ{{<0yv}K1i>!eD67!PDI0T@>#@zK{zKhnVt9M1rU8hjXI`1Oyn z2T|RN*wHep(Z_XjbY7k6XA?uROWn?~yV9Q}W{O>Sx2QTwk};&ikW5qYCJp&6te#tP zp2t1l3zY6FmN3)eQ26Nb6kSo*Pt0*D&D%HhT^1|$c^_4)B{0!?4oU5$EP#xCWK?mg zQdbJbfU?`QvsAll^7S?pT_7(-@Jrs_G|kde8c87@Z%sWIdrY+oNQT5mvQer1Jw`y= zNNKnra<@5Er$`%3Dpyfr{3BU_k|-@LxWqnclSCN1htu?4VF0{BUE!231XFWcT*1b+ zG>>y7nEP}vdoJBcFe>mD;4C4j0H+&tI~CNo#^?vYcHF90;nD5?=GkL|vyUW!w|f=5 z)>~NWbZd09c5RX&b+wK*ecHokAQ>TOh%vXGB`3Fs7j&jr0YMdv?TUPp_6FyHaGs*B zOS3`bWQ_LKm>C97-Ag@F1GzmbyH}KP2;s7v(JVa4f%`KC*N9tBa#l);tTV>u|2$E$jfBy)_$vrhJaQxpVlJX<){ zgJh7&i#jwA-6u7${58e+Kgrpb+5_zw{sv-ndLWq!DN>-L2DB%o5BtsRUM=9(-@I~; z6z~b%Qf$_>m9i*i?*VRQOUljS@vTZ{TC`=3$1oM+*+=J1k;^sryA{{7bM_o__C^u* z=5SiiQW)lVsm=^nAX8CcCjvw&6`GXSsrap^W7S31=IoW)@JQv%1o%;ge@Q{?2aECR zFIkyNZlC`|G5*(8a-RbGKV0%f&FSfih;saNh6thIj$ln&`zR>vNA2V3g+*H<2ad0I zr5k^}E^M|G7J(QD5;$jaVttRj_+@)adlTP`2)T?=EJ$C*9z=D|ND)m{JgHxEYN#8p zGwt)jd0(1zarm6*yLpOU#-jlK2-Ue}4vn9F>?XwTKQs`k0Jm5qL4}c0vP+q2;R+_# zO2(&a!?(P(Y3HwUrPp`X#-5&CnDjM&U4n6tE@Z7Dj{uiYu7kX@C#pCEXrCiKa)Mvu z+sxRQE51nGgW;O4Kg zy#~M%405L`hXL7{)qw9YT#LCBhG!fX%<&jjpKJ8|j8iX7xS$MZ9wXIet8aEe$(Ot# z-C|g7iHUpW0jRa(WxyAx4v2my-Nig~li>xvmdU$M7)TPxBR} z^KO69qvE(PJqGwZulM!QZ?NE%6xpdZ4{?sw(O-6s=-Wh$Z{<>)S1Bz$pcOF5#{ zPPGwkG0pK9!2A!qrHlB9F}+zd#a%@gP$ zF9i?*Ewi)(et|n``;{9SDlM`xkQy + + + + + + 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:2023-12-06 07:59:45Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()27
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()27
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()27
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().227
+
+
+ + + +
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..ca42124f6f --- /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:2023-12-06 07:59:45Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()27
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()27
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()27
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().227
+
+
+ + + +
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..326a8b5e20 --- /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:2023-12-06 07:59:45Functions: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          27 :   GpsBaro() : StateGeneric(estimator_name, is_core_plugin) {
+      15          27 :   }
+      16             : 
+      17          54 :   ~GpsBaro(void) {
+      18          54 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_baro
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          27 : 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:2023-12-06 07:59:45Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()10
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()10
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()10
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().210
+
+
+ + + +
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..12db9d2b86 --- /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:2023-12-06 07:59:45Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()10
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()10
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()10
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().210
+
+
+ + + +
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..6d5ccdfe51 --- /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:2023-12-06 07:59:45Functions: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          10 :   GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15          10 :   }
+      16             : 
+      17          20 :   ~GpsGarmin(void) {
+      18          20 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          10 : 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..8923ea2b55 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html @@ -0,0 +1,182 @@ + + + + + + + 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:30643570.3 %
Date:2023-12-06 07:59:45Functions:243372.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
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..ca5f320fc3 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html @@ -0,0 +1,182 @@ + + + + + + + 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:30643570.3 %
Date:2023-12-06 07:59:45Functions:243372.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
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..2a14b6f127 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail.html @@ -0,0 +1,182 @@ + + + + + + + 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:30643570.3 %
Date:2023-12-06 07:59:45Functions:243372.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
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..8e0851f887 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-f.html @@ -0,0 +1,142 @@ + + + + + + + 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:30643570.3 %
Date:2023-12-06 07:59:45Functions:243372.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
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..496682a5ce --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html @@ -0,0 +1,142 @@ + + + + + + + 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:30643570.3 %
Date:2023-12-06 07:59:45Functions:243372.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
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..6ea28b339f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index.html @@ -0,0 +1,142 @@ + + + + + + + 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:30643570.3 %
Date:2023-12-06 07:59:45Functions:243372.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
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..fe7be3e972 --- /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:2023-12-06 07:59:45Functions: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()3
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&)3
mrs_uav_state_estimators::passthrough::Passthrough::start()3
mrs_uav_state_estimators::passthrough::Passthrough::timerUpdate(ros::TimerEvent const&)4141
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)4191
+
+
+ + + +
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..e8403c9b54 --- /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:2023-12-06 07:59:45Functions:5955.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()3
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&)3
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&)4141
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)4191
mrs_uav_state_estimators::passthrough::Passthrough::pause()0
mrs_uav_state_estimators::passthrough::Passthrough::reset()0
mrs_uav_state_estimators::passthrough::Passthrough::start()3
+
+
+ + + +
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..bff03c6cc8 --- /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:2023-12-06 07:59:45Functions: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           3 : void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16           3 :   ch_ = ch;
+      17           3 :   ph_ = ph;
+      18             : 
+      19           3 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // | --------------- param loader initialization -------------- |
+      22             : 
+      23           3 :   if (is_core_plugin_) {
+      24             : 
+      25           3 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      26           3 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      27             :   }
+      28             : 
+      29           3 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      30             : 
+      31             :   // | --------------------- load parameters -------------------- |
+      32           3 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      33           3 :   ph->param_loader->loadParam("message/topic", msg_topic_);
+      34           3 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+      35             : 
+      36             :   // | ------------------ timers initialization ----------------- |
+      37           3 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this, false, false);  // not running after init
+      38           3 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerCheckHealth, this);
+      39             : 
+      40             :   // | --------------- subscribers initialization --------------- |
+      41           6 :   mrs_lib::SubscribeHandlerOptions shopts;
+      42           3 :   shopts.nh                 = nh;
+      43           3 :   shopts.node_name          = getPrintName();
+      44           3 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      45           3 :   shopts.threadsafe         = true;
+      46           3 :   shopts.autostart          = true;
+      47           3 :   shopts.queue_size         = 10;
+      48           3 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      49             : 
+      50           3 :   sh_passthrough_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_);
+      51             : 
+      52             :   // | ---------------- publishers initialization --------------- |
+      53           3 :   ph_odom_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);  // needed for tf
+      54           3 :   if (ch_->debug_topics.state) {
+      55           3 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      56             :   }
+      57           3 :   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           3 :   if (ch_->debug_topics.innovation) {
+      62           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      63             :   }
+      64           3 :   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           3 :   uav_state_init_.header.frame_id = ns_frame_id_;
+      70           3 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+      71             : 
+      72           3 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+      73           3 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+      74           3 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+      75             : 
+      76           3 :   innovation_init_.header.frame_id         = ns_frame_id_;
+      77           3 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+      78           3 :   innovation_init_.pose.pose.orientation.w = 1.0;
+      79             : 
+      80             :   // | ------------------ finish initialization ----------------- |
+      81             : 
+      82           3 :   if (changeState(INITIALIZED_STATE)) {
+      83           3 :     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           3 : }
+      88             : /*//}*/
+      89             : 
+      90             : /*//{ start() */
+      91           3 : bool Passthrough::start(void) {
+      92             : 
+      93             : 
+      94           3 :   if (isInState(READY_STATE)) {
+      95             : 
+      96           3 :     timer_update_.start();
+      97           3 :     changeState(STARTED_STATE);
+      98           3 :     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        4141 : void Passthrough::timerUpdate(const ros::TimerEvent &event) {
+     140             : 
+     141             : 
+     142        4141 :   if (!isInitialized()) {
+     143           0 :     return;
+     144             :   }
+     145             : 
+     146        4141 :   const ros::Time time_now = ros::Time::now();
+     147             : 
+     148        8282 :   nav_msgs::OdometryConstPtr msg = sh_passthrough_odom_.getMsg();
+     149             : 
+     150        4141 :   if (first_iter_) {
+     151           3 :     prev_msg_   = msg;
+     152           3 :     first_iter_ = false;
+     153             :   }
+     154             : 
+     155        8282 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     156             : 
+     157        4141 :   uav_state.header.stamp = time_now;
+     158             : 
+     159        4141 :   uav_state.pose.position    = msg->pose.pose.position;
+     160        4141 :   uav_state.pose.orientation = msg->pose.pose.orientation;
+     161             : 
+     162        4141 :   uav_state.velocity.linear  = Support::rotateVector(msg->twist.twist.linear, msg->pose.pose.orientation);
+     163        4141 :   uav_state.velocity.angular = msg->twist.twist.angular;
+     164             : 
+     165        8282 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     166             : 
+     167        8282 :   nav_msgs::Odometry innovation = innovation_init_;
+     168        4141 :   innovation.header.stamp       = time_now;
+     169             : 
+     170        4141 :   innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x;
+     171        4141 :   innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y;
+     172        4141 :   innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z;
+     173             : 
+     174        8282 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     175        4141 :   pose_covariance.header.stamp  = time_now;
+     176        4141 :   twist_covariance.header.stamp = time_now;
+     177             : 
+     178        4141 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     179        4141 :   pose_covariance.values.resize(n_states * n_states);
+     180        4141 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     181        4141 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     182        4141 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     183             : 
+     184        4141 :   twist_covariance.values.resize(n_states * n_states);
+     185        4141 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     186        4141 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     187        4141 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     188             : 
+     189        4141 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     190        4141 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     191        4141 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     192        4141 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     193        4141 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     194             : 
+     195        4141 :   publishUavState();
+     196        4141 :   publishOdom();
+     197        4141 :   publishCovariance();
+     198        4141 :   publishInnovation();
+     199        4141 :   publishDiagnostics();
+     200             : 
+     201        4141 :   prev_msg_ = msg;
+     202             : }
+     203             : /*//}*/
+     204             : 
+     205             : /*//{ timerCheckHealth() */
+     206        4191 : void Passthrough::timerCheckHealth(const ros::TimerEvent &event) {
+     207             : 
+     208        4191 :   if (!isInitialized()) {
+     209           0 :     return;
+     210             :   }
+     211             : 
+     212        4191 :   if (isInState(INITIALIZED_STATE)) {
+     213             : 
+     214          48 :     if (sh_passthrough_odom_.hasMsg()) {
+     215           3 :       changeState(READY_STATE);
+     216           3 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     217             :     } else {
+     218          45 :       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          45 :       return;
+     220             :     }
+     221             :   }
+     222             : 
+     223             : 
+     224        4146 :   if (isInState(STARTED_STATE)) {
+     225             : 
+     226           3 :     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(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           3 : 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..952c0015116d8270a63eb96783a0d220ca31918b GIT binary patch literal 1098 zcmV-Q1hxB#P)*X4RlZ_&k1 zGc8g-$K^HUkd6ySr`IJT(GA`oh&Ewanxd7GDkrHx-G6~5!K6tj00plC`J#>X(!ZLw zsSrNL%aJa@XZ^Q?PujuVp%cO3@+GSU5y2#$(u9x;WJJZdhi^4WcLEHi8H$V!$AZIx zBSVXJQiG)>9c6-ZK%ZUsZ5o*YV-}QMC1Y-FAk!4-jEsQ>a6rd7S^`an%hWO0kKVE6 zS`au|U>dp{m>OOLg5_9{or`V)foqIrng<6wkV%-Mq^kL&STF5ZmxcN-aM{yel#OvI zUQAoC9JxhGq+1-&znr%1bad?dStJZR>%e1U(SVL+x%(5*1-X&v<0zZlC7Mr3eY6QV zN4?!l)E<{bc?XaH(L#h6$Ruj}a0>$(K*0n;2{^3d{a?qk1_TU5SbEdEfUmS%^}Vp8 z`A7>Co#mlH0)I%_UThhskjJZ$nsS zw?!e`?S2%>!HN1hAHtSDKj`+DS=QyAxayrz2Rq7Ed@kFzh2bzn9G1oR4cM8e*X zxPGDmow`7y|8VV$K7*BpiKl%_ABs-lOj4(CIX8q&b9F=kW`F(cY)KmR0B zZ~SaHvj*I#>3!g!j-Fpyg;HR}J(NaI{Rpl&f&_c{NJ&tkaFPx;Kgs12z#9gjl6v$7 zu*2$%YHPMb@ QfB*mh07*qoM6N<$f+&#(bpQYW 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..b574ebd33f --- /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:2023-12-06 07:59:45Functions: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..127b708073 --- /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:2023-12-06 07:59:45Functions: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..81494ce6a7 --- /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:2023-12-06 07:59:45Functions: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/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..c44012315f --- /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:2023-12-06 07:59:45Functions: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&)39
mrs_uav_state_estimators::StateGeneric::start()693
mrs_uav_state_estimators::StateGeneric::updateUavState()48022
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)49953
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)49956
mrs_uav_state_estimators::StateGeneric::getHeading() const80521
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()() const80521
+
+
+ + + +
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..79e5d6dffd --- /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:2023-12-06 07:59:45Functions: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&)39
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&)49953
mrs_uav_state_estimators::StateGeneric::updateUavState()48022
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)49956
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()693
mrs_uav_state_estimators::StateGeneric::getHeading() const80521
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()() const80521
+
+
+ + + +
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..999d4c7259 --- /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:2023-12-06 07:59:45Functions: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          39 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13          39 :   ch_ = ch;
+      14          39 :   ph_ = ph;
+      15             : 
+      16          78 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18          39 :   if (is_core_plugin_) {
+      19             : 
+      20          39 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21          39 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24          39 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          39 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27          39 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28          39 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29          39 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31          39 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32          39 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36          78 :   std::string topic_orientation;
+      37          39 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38          39 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39          78 :   std::string topic_angular_velocity;
+      40          39 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41          39 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43          39 :   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          39 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51          39 :   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          39 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56          78 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57          39 :   shopts.nh                 = nh;
+      58          39 :   shopts.node_name          = getPrintName();
+      59          39 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60          39 :   shopts.threadsafe         = true;
+      61          39 :   shopts.autostart          = true;
+      62          39 :   shopts.queue_size         = 10;
+      63          39 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65          39 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66          39 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69          39 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70          39 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72          39 :   if (ch_->debug_topics.state) {
+      73          39 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75          39 :   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          39 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82          39 :   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          78 :   std::vector<double> max_altitudes;
+      88             : 
+      89          39 :   if (is_hdg_passthrough_) {
+      90          39 :     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          39 :   est_hdg_->initialize(nh, ch_, ph_);
+      95          39 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97       80560 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98          39 :   est_lat_->initialize(nh, ch_, ph_);
+      99          39 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101          39 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102          39 :   est_alt_->initialize(nh, ch_, ph_);
+     103          39 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105          39 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108          39 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109          39 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111          39 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112          39 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113          39 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115          39 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116          39 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117          39 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121          39 :   if (changeState(INITIALIZED_STATE)) {
+     122          39 :     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          39 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130         693 : bool StateGeneric::start(void) {
+     131             : 
+     132         693 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136         693 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137         583 :       est_lat_start_successful = true;
+     138             :     } else {
+     139         110 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142         693 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143          10 :       est_alt_start_successful = true;
+     144             :     } else {
+     145         683 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148         693 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149         622 :       timer_pub_attitude_.start();
+     150         622 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152          71 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155         693 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157          39 :       changeState(STARTED_STATE);
+     158          39 :       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         654 :   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       49953 : void StateGeneric::timerUpdate(const ros::TimerEvent &event) {
+     209             : 
+     210             : 
+     211       49953 :   if (!isInitialized()) {
+     212        1931 :     return;
+     213             :   }
+     214             : 
+     215       99394 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     216             : 
+     217       49697 :   switch (getCurrentSmState()) {
+     218             : 
+     219           0 :     case UNINITIALIZED_STATE: {
+     220           0 :       break;
+     221             :     }
+     222         994 :     case INITIALIZED_STATE: {
+     223             : 
+     224         994 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     225          39 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     226          39 :           changeState(READY_STATE);
+     227          39 :           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         955 :         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         955 :         return;
+     235             :       }
+     236             : 
+     237          39 :       break;
+     238             :     }
+     239             : 
+     240         660 :     case READY_STATE: {
+     241         660 :       break;
+     242             :     }
+     243             : 
+     244          60 :     case STARTED_STATE: {
+     245             : 
+     246          60 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     247             : 
+     248          60 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     249          39 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     250          39 :         changeState(RUNNING_STATE);
+     251             :       } else {
+     252          21 :         return;
+     253             :       }
+     254          39 :       break;
+     255             :     }
+     256             : 
+     257       47983 :     case RUNNING_STATE: {
+     258       47983 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     259           0 :         changeState(ERROR_STATE);
+     260             :       }
+     261       47983 :       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       48721 :   if (!isRunning() && !isStarted()) {
+     277         699 :     return;
+     278             :   }
+     279             : 
+     280       48022 :   updateUavState();
+     281             : 
+     282       48022 :   publishUavState();
+     283       48022 :   publishOdom();
+     284       48022 :   publishCovariance();
+     285       48022 :   publishInnovation();
+     286       48022 :   publishDiagnostics();
+     287             : }
+     288             : /*//}*/
+     289             : 
+     290             : /*//{ timerCheckHealth() */
+     291           0 : void StateGeneric::timerCheckHealth(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       49956 : void StateGeneric::timerPubAttitude(const ros::TimerEvent &event) {
+     366             : 
+     367       49956 :   if (!isInitialized()) {
+     368        1313 :     return;
+     369             :   }
+     370             : 
+     371       99400 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     372             : 
+     373       49700 :   if (!sh_hw_api_orient_.hasMsg()) {
+     374         507 :     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         507 :     return;
+     376             :   }
+     377             : 
+     378       49193 :   if (!est_hdg_->isRunning() && !isError()) {
+     379         550 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     380         550 :     return;
+     381             :   }
+     382             : 
+     383       48643 :   scope_timer.checkpoint("checks");
+     384             : 
+     385       48643 :   const ros::Time time_now = ros::Time::now();
+     386             : 
+     387       48643 :   geometry_msgs::QuaternionStamped att;
+     388       48643 :   att.header.stamp    = time_now;
+     389       48643 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     390             : 
+     391             :   double hdg;
+     392       48643 :   if (isError()) {
+     393           0 :     hdg = est_hdg_->getLastValidHdg();
+     394             :   } else {
+     395       48643 :     hdg = est_hdg_->getState(POSITION);
+     396             :   }
+     397             : 
+     398       48643 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     399       48643 :   if (res) {
+     400       48643 :     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       48643 :   scope_timer.checkpoint("rotate");
+     407             : 
+     408       48643 :   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       48643 :   scope_timer.checkpoint("nan check");
+     414             : 
+     415       48643 :   ph_attitude_.publish(att);
+     416       48643 :   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       48022 : void StateGeneric::updateUavState() {
+     432             : 
+     433       48022 :   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       48022 :   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       96044 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     443             : 
+     444       48022 :   const ros::Time time_now = ros::Time::now();
+     445             : 
+     446       48022 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     447       48022 :   uav_state.header.stamp       = time_now;
+     448             : 
+     449             :   // do not rotate orientation if passthrough hdg
+     450       48022 :   if (est_hdg_name_ == "hdg_passthrough") {
+     451           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     452             :   } else {
+     453             :     double hdg;
+     454       48022 :     if (isError()) {
+     455           0 :       hdg = est_hdg_->getLastValidHdg();
+     456             :     } else {
+     457       48022 :       hdg = est_hdg_->getState(POSITION);
+     458             :     }
+     459             : 
+     460       48022 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     461       48022 :     if (res) {
+     462       48022 :       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       48022 :   scope_timer.checkpoint("rotate orientation");
+     470             : 
+     471       48022 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     472             : 
+     473       48022 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     474       48022 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     475       48022 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     476             : 
+     477       48022 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     478       48022 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     479       48022 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     480             : 
+     481       48022 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     482       48022 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     483       48022 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     484             : 
+     485       48022 :   scope_timer.checkpoint("fill uav state");
+     486             : 
+     487       96044 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     488       48022 :   scope_timer.checkpoint("uav state to odom");
+     489             : 
+     490       96044 :   nav_msgs::Odometry innovation = innovation_init_;
+     491       48022 :   innovation.header.stamp       = time_now;
+     492             : 
+     493       48022 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     494       48022 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     495       48022 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     496             : 
+     497       48022 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     498             : 
+     499       48022 :   scope_timer.checkpoint("innovation");
+     500             : 
+     501       96044 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     502       48022 :   pose_covariance.header.stamp  = time_now;
+     503       48022 :   twist_covariance.header.stamp = time_now;
+     504             : 
+     505       48022 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     506       48022 :   pose_covariance.values.resize(n_states * n_states);
+     507       48022 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     508       48022 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     509       48022 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     510             : 
+     511       48022 :   twist_covariance.values.resize(n_states * n_states);
+     512       48022 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     513       48022 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     514       48022 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     515             : 
+     516       48022 :   scope_timer.checkpoint("covariance");
+     517             : 
+     518       48022 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     519       48022 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     520       48022 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     521       48022 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     522       48022 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ getHeading() */
+     527       80521 : std::optional<double> StateGeneric::getHeading() const {
+     528       80521 :   if (!est_hdg_->isRunning()) {
+     529          68 :     return {};
+     530             :   }
+     531       80453 :   return est_hdg_->getState(POSITION);
+     532             : }
+     533             : /*//}*/
+     534             : 
+     535             : /*//{ setUavState() */
+     536           0 : bool StateGeneric::setUavState(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..2900f2b854c8fdbf8391333f47e3b1b1066a5d06 GIT binary patch literal 1971 zcmV;k2Tb^hP)RX!#_;9zBJ-wZR?-u9;XlY|Hk&&Tum9N+(j ztHvAOj#M7a7{|+)BRq}>OP5m~oNnIKE4b%b&FRqos{JR!1dd^vwVw5*fH-N&{yblS zF{npf#N#yAhEi7HC?KH2oiscI%A2_~{h-}4St;CW70}bxmtmYwdN@FP$H*p^>_CGt zufkaE)fh9F?Fx)mTOVd^m-_gwjsV~OsZ{c-gPRi4vk(m+TRVjlT!**Dj;l`v07kW7 z!e#rvysfr|_wKrVKmaQy>}BiJCb8Oa^}G{J$im~1l0()TM^2i@kBljq!c|uHiLJ{> zPYLrN*C0(S4Cg%WJyg4LwmUF7KqpuMFcw?{)DELSJ0)EO)Tl5!($qLQ0D^;%F1ZAA ziW(qFnmEN+cZz9}=_<}+nZYI=qaY@qEY)(jENihwvm&gi-Gu&Q0$J6&~ zku3v`!ElVLWKrrA6^uOrEB%r%aP&h=y0LWXet(Mqsv@0W^&1F%=#m&6EMtoAHXg9+gweS^*rS zn32~glV52 zadp~%l(on8cyTYqQU)+~S)4g76l2u%9XwV8DK?@7q&nu7(yYgKdS0`+s$z_pNk}$` z)R3oUUM zzNom`gywqnT)s6m__udx>1O;l6q*&xXKr7CfDIVSu%`ga*%iK#)2plCmQy9%QCHzu zb^3+pddwMYDztSKfce$6S>SpY!e)}@^>qkf1*uVt_pOi^>4Fhkgo&D;Jl+S5*sTnnZvWBjreQl3SA^C3tLu-K4Y@mcsIRm$x=)~h{? zw8{EJm{u!u3K$~^Qsj3!zZcV?h--2);_<6gb!O*``^s_602X5p5>ZTtS!jO4aqsdT z^CRlHoyVqC+^?Gut-|lTUr4vkm2l@yENb%V*ORBVX#G|9J|aB|O}Qa$0cy(x?LsPl zV(~8Zu#HaZ^&2ZzCHh)tmUXMt-FwY zq3RA-hSGJ<%ggJ|rc00Mnph>9m~!elN)>Ya^n28!Z7BFqJ#@f;GVoV)G;3R>;*O>i zUK%#0Z8g^;9CjV;kG(|uA0O^O!^3Tqw3qWz|HQVPqt{$)HK!G+d`km#Gq^TO7xwPh zIqiNdcL#v>BciD=ix2n9+Ci8uTZ_7tT#Df=o)DY-;ioJ{l=QF#h2uz*j}YmM3%1tf zHC4PHH+v4#nGmd`;Bs#Q%e{Rej+A=?*bTGy*pYIt+ZKd+x#!e_?a*M~l!`w5+=l_n zJqK8y-1pw-<|B9h(d!W!Lss1xS2~F?c1%SvUIM5o>2p$NX{#;TTm@(kj9S2RJm%7e zC4VKLmA1BPoGSt6(>x~A z;JOERNhgudVGMl5rBV!7Fkw!OpL@8g;I`mlA4fp;AM3(H#&XYA$^dR)TF;TGdH|lZ z3{mD>1Kh!O!D)Kx0r-J1*E;St$nz^^8PILl%~Svx9?6wFFi8TO$-L1OK>EChujqV~ zSJ&J6I%VBS%&N+kv-bPf0bAqGK}xy+dBAT_cx$VP?wYdK)tA@Uh>wVtnpc!icUM!T4Yo>L2r{{ZriD^SDq$1eZ?002ovPDHLk FV1hK7w8;Pf literal 0 HcmV?d00001 diff --git a/mrs_uav_testing/src/index-detail-sort-f.html b/mrs_uav_testing/src/index-detail-sort-f.html new file mode 100644 index 0000000000..e3706a4c92 --- /dev/null +++ b/mrs_uav_testing/src/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
test_generic.cpp +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
<unnamed>83.6 %158 / 18984.6 %11 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/index-detail-sort-l.html b/mrs_uav_testing/src/index-detail-sort-l.html new file mode 100644 index 0000000000..58d7ea23c9 --- /dev/null +++ b/mrs_uav_testing/src/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
test_generic.cpp +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
<unnamed>83.6 %158 / 18984.6 %11 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/index-detail.html b/mrs_uav_testing/src/index-detail.html new file mode 100644 index 0000000000..bd79f9b71e --- /dev/null +++ b/mrs_uav_testing/src/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
test_generic.cpp +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
<unnamed>83.6 %158 / 18984.6 %11 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/index-sort-f.html b/mrs_uav_testing/src/index-sort-f.html new file mode 100644 index 0000000000..c92f6053a1 --- /dev/null +++ b/mrs_uav_testing/src/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
test_generic.cpp +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/index-sort-l.html b/mrs_uav_testing/src/index-sort-l.html new file mode 100644 index 0000000000..934934933b --- /dev/null +++ b/mrs_uav_testing/src/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
test_generic.cpp +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/index.html b/mrs_uav_testing/src/index.html new file mode 100644 index 0000000000..9013ef90ec --- /dev/null +++ b/mrs_uav_testing/src/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
test_generic.cpp +
83.6%83.6%
+ 		83.6 %158 / 18984.6 %11 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/test_generic.cpp.func-sort-c.html b/mrs_uav_testing/src/test_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..23d8982230 --- /dev/null +++ b/mrs_uav_testing/src/test_generic.cpp.func-sort-c.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src/test_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/src - test_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_testing::TestGeneric::isOutputEnabled()0
mrs_uav_testing::TestGeneric::hasGoal()0
mrs_uav_testing::TestGeneric::spawnGazeboUav[abi:cxx11]()1
mrs_uav_testing::TestGeneric::gotoAbs[abi:cxx11](double const&, double const&, double const&, double const&)1
mrs_uav_testing::TestGeneric::gotoRel[abi:cxx11](double const&, double const&, double const&, double const&)1
mrs_uav_testing::TestGeneric::activateMidAir[abi:cxx11]()2
mrs_uav_testing::TestGeneric::takeoff[abi:cxx11]()6
mrs_uav_testing::TestGeneric::initialize()9
mrs_uav_testing::TestGeneric::TestGeneric()9
mrs_uav_testing::TestGeneric::isAtPosition(double const&, double const&, double const&, double const&, double const&)106
mrs_uav_testing::TestGeneric::isFlyingNormally()111
mrs_uav_testing::TestGeneric::mrsSystemReady()606
mrs_uav_testing::TestGeneric::sleep(double const&)976
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/test_generic.cpp.func.html b/mrs_uav_testing/src/test_generic.cpp.func.html new file mode 100644 index 0000000000..5f664cdde8 --- /dev/null +++ b/mrs_uav_testing/src/test_generic.cpp.func.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src/test_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/src - test_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_testing::TestGeneric::initialize()9
mrs_uav_testing::TestGeneric::isAtPosition(double const&, double const&, double const&, double const&, double const&)106
mrs_uav_testing::TestGeneric::activateMidAir[abi:cxx11]()2
mrs_uav_testing::TestGeneric::mrsSystemReady()606
mrs_uav_testing::TestGeneric::spawnGazeboUav[abi:cxx11]()1
mrs_uav_testing::TestGeneric::isOutputEnabled()0
mrs_uav_testing::TestGeneric::isFlyingNormally()111
mrs_uav_testing::TestGeneric::sleep(double const&)976
mrs_uav_testing::TestGeneric::gotoAbs[abi:cxx11](double const&, double const&, double const&, double const&)1
mrs_uav_testing::TestGeneric::gotoRel[abi:cxx11](double const&, double const&, double const&, double const&)1
mrs_uav_testing::TestGeneric::hasGoal()0
mrs_uav_testing::TestGeneric::takeoff[abi:cxx11]()6
mrs_uav_testing::TestGeneric::TestGeneric()9
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/test_generic.cpp.gcov.frameset.html b/mrs_uav_testing/src/test_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..f24f21544c --- /dev/null +++ b/mrs_uav_testing/src/test_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src/test_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_testing/src/test_generic.cpp.gcov.html b/mrs_uav_testing/src/test_generic.cpp.gcov.html new file mode 100644 index 0000000000..b562ebc517 --- /dev/null +++ b/mrs_uav_testing/src/test_generic.cpp.gcov.html @@ -0,0 +1,588 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src/test_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_testing/src - test_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:15818983.6 %
Date:2023-12-06 07:59:45Functions:111384.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_testing/test_generic.h>
+       2             : 
+       3             : namespace mrs_uav_testing
+       4             : {
+       5             : 
+       6             : /* constructors //{ */
+       7             : 
+       8           9 : TestGeneric::TestGeneric() {
+       9             : 
+      10           9 :   initialize();
+      11           9 : }
+      12             : 
+      13             : //}
+      14             : 
+      15             : /* initialize() //{ */
+      16             : 
+      17           9 : void TestGeneric::initialize(void) {
+      18             : 
+      19           9 :   nh_ = ros::NodeHandle("~");
+      20             : 
+      21           9 :   ROS_INFO("[%s]: ROS node initialized", name_.c_str());
+      22             : 
+      23           9 :   ros::Time::waitForValid();
+      24             : 
+      25           9 :   spinner_ = make_shared<ros::AsyncSpinner>(4);
+      26           9 :   spinner_->start();
+      27             : 
+      28             :   // | ----------------------- load params ---------------------- |
+      29             : 
+      30          18 :   mrs_lib::ParamLoader param_loader(nh_, "Test");
+      31             : 
+      32           9 :   param_loader.loadParam("uav_name", _uav_name_, std::string());
+      33           9 :   param_loader.loadParam("test_name", _test_name_, std::string());
+      34           9 :   param_loader.loadParam("gazebo_spawner_params", _gazebo_spawner_params_, std::string());
+      35             : 
+      36           9 :   name_ = _uav_name_ + "/" + _test_name_;
+      37             : 
+      38             :   // | ----------------------- subscribers ---------------------- |
+      39             : 
+      40          18 :   mrs_lib::SubscribeHandlerOptions shopts;
+      41           9 :   shopts.nh                 = nh_;
+      42           9 :   shopts.node_name          = name_;
+      43           9 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      44           9 :   shopts.threadsafe         = true;
+      45           9 :   shopts.autostart          = true;
+      46           9 :   shopts.queue_size         = 10;
+      47           9 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      48             : 
+      49           9 :   sh_control_manager_diag_    = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "/" + _uav_name_ + "/control_manager/diagnostics");
+      50           9 :   sh_uav_manager_diag_        = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts, "/" + _uav_name_ + "/uav_manager/diagnostics");
+      51           9 :   sh_estim_manager_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "/" + _uav_name_ + "/estimation_manager/diagnostics");
+      52           9 :   sh_constraint_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics>(shopts, "/" + _uav_name_ + "/constraint_manager/diagnostics");
+      53           9 :   sh_gain_manager_diag_       = mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>(shopts, "/" + _uav_name_ + "/gain_manager/diagnostics");
+      54           9 :   sh_uav_state_               = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "/" + _uav_name_ + "/estimation_manager/uav_state");
+      55           9 :   sh_gazebo_spawner_diag_     = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts, "/mrs_drone_spawner/diagnostics");
+      56             : 
+      57           9 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "/" + _uav_name_ + "/hw_api/status");
+      58             : 
+      59             :   // | --------------------- service clients -------------------- |
+      60             : 
+      61           9 :   sch_arming_            = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "/" + _uav_name_ + "/hw_api/arming");
+      62           9 :   sch_offboard_          = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/hw_api/offboard");
+      63           9 :   sch_midair_activation_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/uav_manager/midair_activation");
+      64           9 :   sch_spawn_gazebo_uav_  = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "/mrs_drone_spawner/spawn");
+      65             : 
+      66           9 :   sch_goto_          = mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>(nh_, "/" + _uav_name_ + "/control_manager/goto");
+      67           9 :   sch_goto_relative_ = mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>(nh_, "/" + _uav_name_ + "/control_manager/goto_relative");
+      68             : 
+      69             :   // | --------------------- finish the init -------------------- |
+      70             : 
+      71           9 :   ROS_INFO("[%s]: initialized", name_.c_str());
+      72           9 : }
+      73             : 
+      74             : //}
+      75             : 
+      76             : /* spawnGazeboUav() //{ */
+      77             : 
+      78           4 : tuple<bool, string> TestGeneric::spawnGazeboUav() {
+      79             : 
+      80             :   // | ------------ wait for the spawner to be ready ------------ |
+      81             : 
+      82             :   while (true) {
+      83             : 
+      84           4 :     if (!ros::ok()) {
+      85           0 :       return {false, "shut down from outside"};
+      86             :     }
+      87             : 
+      88           4 :     ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the Gazebo drone spawner", name_.c_str());
+      89             : 
+      90           4 :     if (sh_gazebo_spawner_diag_.hasMsg()) {
+      91           1 :       break;
+      92             :     }
+      93             : 
+      94           3 :     sleep(0.1);
+      95           3 :   }
+      96             : 
+      97             :   // | -------------------------- wait  ------------------------- |
+      98             : 
+      99           1 :   sleep(1.0);
+     100             : 
+     101             :   // | ------------------- call spawn service ------------------- |
+     102             : 
+     103             :   {
+     104             : 
+     105           1 :     mrs_msgs::String srv;
+     106             : 
+     107           1 :     srv.request.value = _gazebo_spawner_params_;
+     108             : 
+     109           1 :     bool service_call = sch_spawn_gazebo_uav_.call(srv);
+     110             : 
+     111           1 :     if (!service_call || !srv.response.success) {
+     112           0 :       return {false, "gazebo drone spawner service call failed"};
+     113             :     }
+     114             :   }
+     115             : 
+     116             :   // | ------------------ wait while processing ----------------- |
+     117             : 
+     118             :   while (true) {
+     119             : 
+     120           1 :     if (!ros::ok()) {
+     121           0 :       return {false, "shut down from outside"};
+     122             :     }
+     123             : 
+     124           1 :     ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the drone to spawn", name_.c_str());
+     125             : 
+     126           1 :     if (!sh_gazebo_spawner_diag_.getMsg()->processing) {
+     127           1 :       break;
+     128             :     }
+     129             : 
+     130           0 :     sleep(0.1);
+     131           0 :   }
+     132             : 
+     133           1 :   return {true, "drone spawned"};
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* sleep() //{ */
+     139             : 
+     140         976 : void TestGeneric::sleep(const double &duration) {
+     141             : 
+     142         976 :   ros::Duration(duration).sleep();
+     143         976 : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* takeoff() //{ */
+     148             : 
+     149         419 : tuple<bool, string> TestGeneric::takeoff(void) {
+     150             : 
+     151             :   // | ---------------- wait for ready to takeoff --------------- |
+     152             : 
+     153             :   while (true) {
+     154             : 
+     155         419 :     if (!ros::ok()) {
+     156           0 :       return {false, "shut down from outside"};
+     157             :     }
+     158             : 
+     159         419 :     ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str());
+     160             : 
+     161         419 :     if (mrsSystemReady()) {
+     162           6 :       ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str());
+     163           6 :       break;
+     164             :     }
+     165             : 
+     166         413 :     sleep(0.1);
+     167         413 :   }
+     168             : 
+     169             :   // | ---------------------- arm the drone --------------------- |
+     170             : 
+     171           6 :   ROS_INFO("[%s]: arming the drone", name_.c_str());
+     172             : 
+     173             :   {
+     174           6 :     std_srvs::SetBool srv;
+     175           6 :     srv.request.data = true;
+     176             : 
+     177             :     {
+     178           6 :       bool service_call = sch_arming_.call(srv);
+     179             : 
+     180           6 :       if (!service_call || !srv.response.success) {
+     181           0 :         return {false, "arming service call failed"};
+     182             :       }
+     183             :     }
+     184             :   }
+     185             : 
+     186             :   // | ---------------------- wait a second --------------------- |
+     187             : 
+     188           6 :   sleep(2.0);
+     189             : 
+     190             :   // | --------------------- check if armed --------------------- |
+     191             : 
+     192           6 :   if (!sh_hw_api_status_.getMsg()->armed) {
+     193           3 :     return {false, "not armed"};
+     194             :   }
+     195             : 
+     196             :   // | ------------------- switch to offboard ------------------- |
+     197             : 
+     198             :   {
+     199           3 :     std_srvs::Trigger srv;
+     200             : 
+     201             :     {
+     202           3 :       bool service_call = sch_offboard_.call(srv);
+     203             : 
+     204           3 :       if (!service_call || !srv.response.success) {
+     205           0 :         return {false, "offboard service call failed"};
+     206             :       }
+     207             :     }
+     208             :   }
+     209             : 
+     210             :   // | -------------------------- wait -------------------------- |
+     211             : 
+     212           3 :   sleep(0.1);
+     213             : 
+     214             :   // | ------------------ check if in offboard ------------------ |
+     215             : 
+     216           3 :   if (!sh_hw_api_status_.getMsg()->offboard) {
+     217           0 :     return {false, "not in offboard"};
+     218             :   }
+     219             : 
+     220             :   // | --------------- wait for takeoff to finish --------------- |
+     221             : 
+     222             :   while (true) {
+     223             : 
+     224         308 :     if (!ros::ok()) {
+     225           0 :       return {false, "shut down from outside"};
+     226             :     }
+     227             : 
+     228         308 :     ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the takeoff to finish", name_.c_str());
+     229             : 
+     230         308 :     if (sh_control_manager_diag_.getMsg()->flying_normally) {
+     231             : 
+     232           3 :       return {true, "takeoff finished"};
+     233             :     }
+     234             : 
+     235         305 :     sleep(0.1);
+     236         305 :   }
+     237             : 
+     238             :   return {false, "reached end of the method without assertion"};
+     239             : }
+     240             : 
+     241             : //}
+     242             : 
+     243             : /* activateMidAir() //{ */
+     244             : 
+     245         124 : tuple<bool, string> TestGeneric::activateMidAir(void) {
+     246             : 
+     247             :   // | ---------------- wait for ready to takeoff --------------- |
+     248             : 
+     249             :   while (true) {
+     250             : 
+     251         124 :     if (!ros::ok()) {
+     252           0 :       return {false, "shut down from outside"};
+     253             :     }
+     254             : 
+     255         124 :     ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str());
+     256             : 
+     257         124 :     if (mrsSystemReady()) {
+     258           2 :       ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str());
+     259           2 :       break;
+     260             :     }
+     261             : 
+     262         122 :     sleep(0.1);
+     263         122 :   }
+     264             : 
+     265             :   // | ---------------------- arm the drone --------------------- |
+     266             : 
+     267           2 :   ROS_INFO("[%s]: arming the drone", name_.c_str());
+     268             : 
+     269             :   {
+     270           2 :     std_srvs::SetBool srv;
+     271           2 :     srv.request.data = true;
+     272             : 
+     273             :     {
+     274           2 :       bool service_call = sch_arming_.call(srv);
+     275             : 
+     276           2 :       if (!service_call || !srv.response.success) {
+     277           0 :         return {false, "arming service call failed"};
+     278             :       }
+     279             :     }
+     280             :   }
+     281             : 
+     282             :   // | -------------------------- wait -------------------------- |
+     283             : 
+     284           2 :   sleep(0.1);
+     285             : 
+     286             :   // | --------------------- check if armed --------------------- |
+     287             : 
+     288           2 :   if (!sh_hw_api_status_.getMsg()->armed) {
+     289           0 :     return {false, "not armed"};
+     290             :   }
+     291             : 
+     292             :   // | ----------------- call midair activation ----------------- |
+     293             : 
+     294             :   {
+     295           2 :     std_srvs::Trigger srv;
+     296             : 
+     297             :     {
+     298           2 :       bool service_call = sch_midair_activation_.call(srv);
+     299             : 
+     300           2 :       if (!service_call || !srv.response.success) {
+     301           0 :         return {false, "midair activation service call failed"};
+     302             :       }
+     303             :     }
+     304             :   }
+     305             : 
+     306             :   // | --------------- wait for takeoff to finish --------------- |
+     307             : 
+     308             :   while (true) {
+     309             : 
+     310           4 :     if (!ros::ok()) {
+     311           0 :       return {false, "shut down from outside"};
+     312             :     }
+     313             : 
+     314           4 :     ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the midair activation to finish", name_.c_str());
+     315             : 
+     316           4 :     if (sh_control_manager_diag_.getMsg()->flying_normally) {
+     317             : 
+     318           2 :       return {true, "midair activation finished"};
+     319             :     }
+     320             : 
+     321           2 :     sleep(0.1);
+     322           2 :   }
+     323             : 
+     324             :   return {false, "reached end of the method without assertion"};
+     325             : }
+     326             : 
+     327             : //}
+     328             : 
+     329             : /* mrsSystemReady() //{ */
+     330             : 
+     331         606 : bool TestGeneric::mrsSystemReady(void) {
+     332             : 
+     333         606 :   bool got_control_manager_diag    = sh_control_manager_diag_.hasMsg();
+     334         606 :   bool got_estimation_manager_diag = sh_estim_manager_diag_.hasMsg();
+     335         606 :   bool got_uav_manager_diag        = sh_uav_manager_diag_.hasMsg();
+     336         606 :   bool got_gain_manager_diag       = sh_gain_manager_diag_.hasMsg();
+     337         606 :   bool got_constraint_manager_diag = sh_constraint_manager_diag_.hasMsg();
+     338             : 
+     339         606 :   return got_control_manager_diag && got_estimation_manager_diag && got_uav_manager_diag && got_gain_manager_diag && got_constraint_manager_diag;
+     340             : }
+     341             : 
+     342             : //}
+     343             : 
+     344             : /* isFlyingNormally() //{ */
+     345             : 
+     346         111 : bool TestGeneric::isFlyingNormally(void) {
+     347             : 
+     348         111 :   if (sh_control_manager_diag_.hasMsg()) {
+     349         111 :     return sh_control_manager_diag_.getMsg()->flying_normally;
+     350             :   } else {
+     351           0 :     return false;
+     352             :   }
+     353             : }
+     354             : 
+     355             : //}
+     356             : 
+     357             : /* isOutputEnabled() //{ */
+     358             : 
+     359           0 : bool TestGeneric::isOutputEnabled(void) {
+     360             : 
+     361           0 :   if (sh_control_manager_diag_.hasMsg()) {
+     362           0 :     return sh_control_manager_diag_.getMsg()->output_enabled;
+     363             :   } else {
+     364           0 :     return false;
+     365             :   }
+     366             : }
+     367             : 
+     368             : //}
+     369             : 
+     370             : /* hasGoal() //{ */
+     371             : 
+     372           0 : bool TestGeneric::hasGoal(void) {
+     373             : 
+     374           0 :   if (sh_control_manager_diag_.hasMsg()) {
+     375           0 :     return sh_control_manager_diag_.getMsg()->tracker_status.have_goal;
+     376             :   } else {
+     377           0 :     return false;
+     378             :   }
+     379             : }
+     380             : 
+     381             : //}
+     382             : 
+     383             : /* gotoAbs() //{ */
+     384             : 
+     385           1 : tuple<bool, string> TestGeneric::gotoAbs(const double &x, const double &y, const double &z, const double &hdg) {
+     386             : 
+     387           2 :   mrs_msgs::Vec4 srv;
+     388             : 
+     389           1 :   srv.request.goal[0] = x;
+     390           1 :   srv.request.goal[1] = y;
+     391           1 :   srv.request.goal[2] = z;
+     392           1 :   srv.request.goal[3] = hdg;
+     393             : 
+     394             :   {
+     395           1 :     bool service_call = sch_goto_.call(srv);
+     396             : 
+     397           1 :     if (!service_call || !srv.response.success) {
+     398           0 :       return {false, "goto service call failed"};
+     399             :     }
+     400             :   }
+     401             : 
+     402             :   // | -------------------- check for result -------------------- |
+     403             : 
+     404             :   while (true) {
+     405             : 
+     406          67 :     if (!ros::ok()) {
+     407           0 :       return {false, "shut down from outside"};
+     408             :     }
+     409             : 
+     410          67 :     if (!isFlyingNormally()) {
+     411           0 :       return {false, "not flying normally"};
+     412             :     }
+     413             : 
+     414          67 :     auto diag = sh_estim_manager_diag_.getMsg();
+     415             : 
+     416          67 :     auto current_hdg = mrs_lib::AttitudeConverter(diag->pose.orientation).getHeading();
+     417             : 
+     418          67 :     if (abs(x - diag->pose.position.x) < 0.1 && abs(y - diag->pose.position.y) < 0.1 && abs(z - diag->pose.position.z) < 0.1 && abs(hdg - current_hdg) < 0.1) {
+     419             : 
+     420           0 :       return {true, "goal reached"};
+     421             :     }
+     422             : 
+     423          67 :     if (isAtPosition(x, y, z, hdg, 0.1)) {
+     424           1 :       return {true, "goal reached"};
+     425             :     }
+     426             : 
+     427          66 :     ros::Duration(0.1).sleep();
+     428          66 :   }
+     429             : 
+     430             :   return {false, "reached end of the method without assertion"};
+     431             : }
+     432             : 
+     433             : //}
+     434             : 
+     435             : /* gotoRel() //{ */
+     436             : 
+     437           1 : tuple<bool, string> TestGeneric::gotoRel(const double &x, const double &y, const double &z, const double &hdg) {
+     438             : 
+     439           1 :   auto start_pose = sh_estim_manager_diag_.getMsg()->pose.position;
+     440           1 :   auto start_hdg  = mrs_lib::AttitudeConverter(sh_estim_manager_diag_.getMsg()->pose.orientation).getHeading();
+     441             : 
+     442             :   {
+     443           1 :     mrs_msgs::Vec4 srv;
+     444             : 
+     445           1 :     srv.request.goal[0] = x;
+     446           1 :     srv.request.goal[1] = y;
+     447           1 :     srv.request.goal[2] = z;
+     448           1 :     srv.request.goal[3] = hdg;
+     449             : 
+     450             :     {
+     451           1 :       bool service_call = sch_goto_relative_.call(srv);
+     452             : 
+     453           1 :       if (!service_call || !srv.response.success) {
+     454           0 :         return {false, "goto relative service call failed"};
+     455             :       }
+     456             :     }
+     457             :   }
+     458             : 
+     459             :   // | -------------------- check for result -------------------- |
+     460             : 
+     461             :   while (true) {
+     462             : 
+     463          39 :     if (!ros::ok()) {
+     464           0 :       return {false, "shut down from outside"};
+     465             :     }
+     466             : 
+     467          39 :     if (!isFlyingNormally()) {
+     468           0 :       return {false, "not flying normally"};
+     469             :     }
+     470             : 
+     471          39 :     if (isAtPosition(start_pose.x + x, start_pose.y + y, start_pose.z + z, start_hdg + hdg, 0.1)) {
+     472           1 :       return {true, "goal reached"};
+     473             :     }
+     474             : 
+     475          38 :     ros::Duration(0.1).sleep();
+     476             :   }
+     477             : 
+     478             :   return {false, "reached end of the method without assertion"};
+     479             : }
+     480             : 
+     481             : //}
+     482             : 
+     483             : /* isAtPosition() //{ */
+     484             : 
+     485         106 : bool TestGeneric::isAtPosition(const double &x, const double &y, const double &z, const double &hdg, const double &pos_tolerance) {
+     486             : 
+     487         212 :   auto uav_state = sh_uav_state_.getMsg();
+     488             : 
+     489         106 :   auto current_hdg = mrs_lib::AttitudeConverter(uav_state->pose.orientation).getHeading();
+     490             : 
+     491         121 :   if (abs(x - uav_state->pose.position.x) < pos_tolerance && abs(y - uav_state->pose.position.y) < pos_tolerance &&
+     492         121 :       abs(z - uav_state->pose.position.z) < pos_tolerance && abs(hdg - current_hdg) < 0.1) {
+     493             : 
+     494           2 :     return true;
+     495             : 
+     496             :   } else {
+     497             : 
+     498         104 :     return false;
+     499             :   }
+     500             : }
+     501             : 
+     502             : //}
+     503             : 
+     504             : }  // namespace mrs_uav_testing
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_testing/src/test_generic.cpp.gcov.overview.html b/mrs_uav_testing/src/test_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..39726e454d --- /dev/null +++ b/mrs_uav_testing/src/test_generic.cpp.gcov.overview.html @@ -0,0 +1,146 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_testing/src/test_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 + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_testing/src/test_generic.cpp.gcov.png b/mrs_uav_testing/src/test_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8d53eac79630f0dab64a5101ace0ad5c62a63320 GIT binary patch literal 1400 zcmV-;1&8{HP)9a5*8`gQ#TqG7-uMaSt&~AC|n4~4%aH% zgsa%UjJQVXHjXKeCK;DVm%#G9sY@KGQ}15kXOvN91MkVic0*4+i~k{BO|8Nm(_ec`n)Fr}d%N%&6gJo=TH!hU!t``Dv9bueR zN)%fnQb&aerQt>uTGR1ktTf@Es+6(Ig5{WJ4GQ9A#*2AApWC**)c0$9tIyl}8I7GV zc?J%ajBYH(ND{~%zb^VQCy~xEVuuxQa9m3uQxnpnhP~=?%|)EK9&#N%Hw3>o?FuKz zDB=3#)$mop)jdf$$6c~(XF0BQSKoQtI4%Ikm}^aN2|vn`_zF{sN-O63?dKLoN;#uF zGkRfQa`)8WW_Jz`bGY&n?I~?J-0+Vb_pfre)Hk5uRN*vzAC@5!&Q8xuF89*W9(kOE z_I3c(k8#+tb6V=yooQA$dFr#^bx)03J%_tY(kZP%02*K)=-TV!->I1@Ba)gG#BNK? zxrOW0ysjRe_gzFpgruo|7Kcx&1u%I}ntA|>zwbu6WynaC!Go?@OZupJ>2t0?7IH?= zaQ)OX5e>_YJw(GX40Z^BlmY0gJ{L8s>^ydH+|vq3vw1Yim;zFytDpmKQK=4J3^k?D zM_4vLWIO6JWiulIkXPLp*Xgkfb?Uh?uF=50U|c`Kj`SDCwTx&93Z0yN;u<>RA3RIH zm9;n6gyeFMKHREDg%eb0-(Z-h$6T*j1-i-~UHTM(CV08W3a6PPgg*s_-QnaUoqG&= zzE;K)vfhP{ui|LC>+A1HDR+H6-1T)y#=E}GFXpbVzaNKxxV{c>I0nL>XX9}H5^2kM z;g##_$f-UP*V5=_hreoE-&I^g6c%m_*;4LnsyJaJO*g||gIsqNml?^MB*?oYtYhim zq#L-&Dj-7OCaVBA9?xPz0Zt7NuCR>oC0g#PqXYhE0=-LJ=k`NaKLgwh^fQja$sAW` z_Ru3Fp~$|x<_OI;Q`$u6Xzx3a zv<`2Xy6!eN+Ftvf+uW+x8@}dy*VjxLQJe?J__paBnEIR^z*2arfgK9wjqA*7i2Q?` zK*r;>X%Ro8bF>P7t2P+c{5;nwgj~ad7Zt1Ndu3rq0 z0vIENiy;5QZ|&Fg@q7Dq&p&#VHgS`!T0Y?K*8#W&3Ar2#c%3CYA5_!OB<)MtZnjA-E=W!@T(S1OEX_bh$5YWOA$k0000 + + + + + + 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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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 +
44.4%44.4%
+ 		44.4 %63 / 14238.9 %7 / 18
<unnamed>44.4 %63 / 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..d2b2e70c34 --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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 +
44.4%44.4%
+ 		44.4 %63 / 14238.9 %7 / 18
<unnamed>44.4 %63 / 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..0915f8f221 --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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 +
44.4%44.4%
+ 		44.4 %63 / 14238.9 %7 / 18
<unnamed>44.4 %63 / 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..e372bb0f0a --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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 +
44.4%44.4%
+ 		44.4 %63 / 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..9e9c6198ca --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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 +
44.4%44.4%
+ 		44.4 %63 / 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..e6e0b0c35c --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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 +
44.4%44.4%
+ 		44.4 %63 / 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..bed83094b5 --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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&)1
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()6
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
+
+
+ + + +
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..5643815629 --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()6
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>)42
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&)123
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
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&)1
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&)39318
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..94d704b178 --- /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:6314244.4 %
Date:2023-12-06 07:59:45Functions: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          42 : 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          42 :   this->common_handlers_  = common_handlers;
+     136          42 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138          42 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140          42 :   nh_ = nh;
+     141             : 
+     142          42 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154          42 :   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          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164          84 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184          42 :   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          42 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195          42 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196          42 :   shopts.nh              = nh_;
+     197          42 :   shopts.node_name       = "JoyTracker";
+     198          42 :   shopts.queue_size      = 1;
+     199          42 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201          42 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205          42 :   last_update = ros::Time(0);
+     206             : 
+     207          42 :   is_initialized_ = true;
+     208             : 
+     209          42 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211          42 :   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           6 : void JoyTracker::deactivate(void) {
+     277             : 
+     278           6 :   is_active_ = false;
+     279             : 
+     280           6 :   ROS_INFO("[JoyTracker]: deactivated");
+     281           6 : }
+     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       39318 : 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      117954 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300      117954 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303       39318 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305       39318 :     uav_state_ = uav_state;
+     306             : 
+     307       39318 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310       39318 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312       39318 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315       39318 :   if (!is_active_) {
+     316       39318 :     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           2 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389           4 :   std_srvs::SetBoolResponse res;
+     390           4 :   std::stringstream         ss;
+     391             : 
+     392           2 :   if (cmd->data != callbacks_enabled_) {
+     393             : 
+     394           0 :     callbacks_enabled_ = cmd->data;
+     395             : 
+     396           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     397           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     398             : 
+     399             :   } else {
+     400             : 
+     401           2 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402           2 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405           2 :   res.message = ss.str();
+     406           2 :   res.success = true;
+     407             : 
+     408           4 :   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         123 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467         123 :   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           1 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr JoyTracker::setTrajectoryReference([
+     493             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     494           1 :   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          42 : 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..459ec290a5b8cfaca02a4165454b04bbfed94363 GIT binary patch literal 1757 zcmV<31|s>1P)A#f)d`UhfTfBMfu4iYGNq)W#%R;i5on|%9vp&Bj zsd4h%q;rch0Gm*@Xg9PhBNaF(HN#jj1Oe%c71|7t%1AbkuHzu-a15l3>Q++85#MX@ zCY9oR#%fWcCu6i~uUImWDO#&$@VbeSll{QVxbSJM6>-Txbc6c=(aJ>Rt$_@qHRYOY zjiy`2KPRMSW^VvU{q{WfV8TFs$;~+S*&G0qSq-7zkekG6%}|P2|#Uiq#I2| z2LTM9QWz`4s@Dc;p4Agvm3q9mcE*q{Aaq8jg~g@)1lElNz=miIiV-TX6+N_KWTw^_n6bX= zf)Zp+P1=YvZY-fcaGN!Tf746LUzp^!>OA8En*|D39e${OH!9!B2SNkk2nS?O;q3Q$aN#pFh1J0TGUnQ39Q$*PsXUOuSQ?03l5rJhvIsg|c@T8<#z3{DWHurRg|00kcJ*7l@jgZscmd)YV&x{M7 zntPa5-<-7yw(s)c;(VNOsa3%aNGQP#kUSnaM3y|UI|oRi=QzGmTJ>Y1WydR#x{n<7 zq|0ISn(RW1C*@&vtX&)Y5LOK`PwSwI8s~Cp!;#Vy-*GYU`*;bS0{b_!|_nvW3O3ui&lke*Zr+rMN7?( z!I%&g1*@rk1gop&O-4}I?5y36L&>vAZg9$@?vqo9s2yzFxm*%?RG?p6Qd(?uJ`Y z^)Oq)N9m%$u7a1Abnh!#Grr5sw)u?Ko;ou?H6UnY75>2UtI8s0GaAk+keE|=Q@DVw zT@X4*7B@fEP71k<=c3W3DpdYJL%9Q8TNq#UV%YkOPi|7tcRUb`5qCJFCB5?=AwNF4 z>#D7P-3db9zFEB0jQcrcHPINI>FAGDMmAG@H*?grobo-~NEZj=(UKltSCSv0+XsvhI&lxzYQR?580s(tL@lV(@Fx@FaGBg z-w3;U%!-=)huUV>e;&#LixVAY?;d{uyz(nn#4-{T00000NkvXXu0mjfg-BK1 literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..0e327a2ca8 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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 +
71.6%71.6%
+ 		71.6 %424 / 59267.7 %21 / 31
<unnamed>71.6 %424 / 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..3077705a25 --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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 +
71.6%71.6%
+ 		71.6 %424 / 59267.7 %21 / 31
<unnamed>71.6 %424 / 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..36e58eef0b --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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 +
71.6%71.6%
+ 		71.6 %424 / 59267.7 %21 / 31
<unnamed>71.6 %424 / 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..917a397f48 --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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 +
71.6%71.6%
+ 		71.6 %424 / 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..92bd7cacea --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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 +
71.6%71.6%
+ 		71.6 %424 / 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..4e23c45c6c --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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 +
71.6%71.6%
+ 		71.6 %424 / 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..964f038eb5 --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)3
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)6
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)14
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()20
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)43
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)54
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)66
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()132
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()132
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()716
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()1344
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()5328
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()6672
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)7160
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
+
+
+ + + +
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..3fff49ea26 --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()20
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>)42
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)43
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> >&)2
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> >&)3
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()6672
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)6
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()5328
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()1344
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()132
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)66
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()132
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)54
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)1
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&)39318
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)14
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()716
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)7160
+
+
+ + + +
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..9a2458073d --- /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:42459271.6 %
Date:2023-12-06 07:59:45Functions: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          42 : 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          42 :   this->common_handlers_  = common_handlers;
+     215          42 :   this->private_handlers_ = private_handlers;
+     216             : 
+     217          42 :   _uav_name_ = common_handlers->uav_name;
+     218             : 
+     219          42 :   nh_ = nh;
+     220             : 
+     221          42 :   ros::Time::waitForValid();
+     222             : 
+     223             :   // --------------------------------------------------------------
+     224             :   // |                     loading parameters                     |
+     225             :   // --------------------------------------------------------------
+     226             : 
+     227             :   // | ---------- loading params using the parent's nh ---------- |
+     228             : 
+     229          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     230             : 
+     231          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     232             : 
+     233          42 :   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          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/landoff_tracker.yaml");
+     241          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/landoff_tracker.yaml");
+     242             : 
+     243          84 :   const std::string yaml_prefix = "mrs_uav_trackers/landoff_tracker/";
+     244             : 
+     245          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_speed", _horizontal_speed_);
+     246          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_acceleration", _horizontal_acceleration_);
+     247             : 
+     248          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     249          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_acceleration", _vertical_acceleration_);
+     250             : 
+     251          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_speed", _takeoff_speed_);
+     252          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_acceleration", _takeoff_acceleration_);
+     253             : 
+     254          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_speed", _landing_speed_);
+     255          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_acceleration", _landing_acceleration_);
+     256             : 
+     257          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_speed", _elanding_speed_);
+     258          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_acceleration", _elanding_acceleration_);
+     259             : 
+     260          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     261          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_gain", _heading_gain_);
+     262             : 
+     263          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "main_timer_rate", _main_timer_rate_);
+     264             : 
+     265          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "landing_reference", _landing_reference_);
+     266             : 
+     267          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_position_difference", _max_position_difference_);
+     268             : 
+     269          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains", _takeoff_disable_lateral_gains_);
+     270          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains_z", _takeoff_disable_lateral_gains_z_);
+     271             : 
+     272          42 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     273           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     274           0 :     return false;
+     275             :   }
+     276             : 
+     277          42 :   _tracker_dt_ = 1.0 / double(_main_timer_rate_);
+     278             : 
+     279          42 :   ROS_INFO("[LandoffTracker]: tracker_dt: %f", _tracker_dt_);
+     280             : 
+     281          42 :   state_x_       = 0;
+     282          42 :   state_y_       = 0;
+     283          42 :   state_z_       = 0;
+     284          42 :   state_heading_ = 0;
+     285             : 
+     286          42 :   speed_x_       = 0;
+     287          42 :   speed_y_       = 0;
+     288          42 :   speed_heading_ = 0;
+     289             : 
+     290          42 :   current_horizontal_speed_ = 0;
+     291          42 :   current_vertical_speed_   = 0;
+     292             : 
+     293          42 :   current_horizontal_acceleration_ = 0;
+     294          42 :   current_vertical_acceleration_   = 0;
+     295             : 
+     296          42 :   current_vertical_direction_ = 0;
+     297             : 
+     298          42 :   current_state_vertical_  = LANDED_STATE;
+     299          42 :   previous_state_vertical_ = LANDED_STATE;
+     300             : 
+     301          42 :   current_state_horizontal_  = LANDED_STATE;
+     302          42 :   previous_state_horizontal_ = LANDED_STATE;
+     303             : 
+     304             :   // | ------------------------ profiler ------------------------ |
+     305             : 
+     306          42 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "LandoffTracker", _profiler_enabled_);
+     307             : 
+     308             :   // | ------------------------ services ------------------------ |
+     309             : 
+     310          42 :   service_takeoff_ = nh_.advertiseService("takeoff", &LandoffTracker::callbackTakeoff, this);
+     311          42 :   service_land_    = nh_.advertiseService("land", &LandoffTracker::callbackLand, this);
+     312          42 :   service_eland_   = nh_.advertiseService("eland", &LandoffTracker::callbackELand, this);
+     313             : 
+     314             :   // | ------------------------- timers ------------------------- |
+     315             : 
+     316          42 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &LandoffTracker::timerMain, this, false, false);
+     317             : 
+     318             :   // | ----------------------- finish init ---------------------- |
+     319             : 
+     320          42 :   is_initialized_ = true;
+     321             : 
+     322          42 :   ROS_INFO("[LandoffTracker]: initialized");
+     323             : 
+     324          42 :   return true;
+     325             : }
+     326             : 
+     327             : //}
+     328             : 
+     329             : /* //{ activate() */
+     330             : 
+     331          14 : std::tuple<bool, std::string> LandoffTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     332             : 
+     333          28 :   std::stringstream ss;
+     334             : 
+     335          14 :   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          28 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     344             : 
+     345             :   double uav_heading;
+     346             : 
+     347             :   try {
+     348          14 :     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          28 :     std::scoped_lock lock(mutex_goal_);
+     363             : 
+     364             :     // the last command is usable
+     365          14 :     state_x_       = uav_state.pose.position.x;
+     366          14 :     state_y_       = uav_state.pose.position.y;
+     367          14 :     state_z_       = uav_state.pose.position.z;
+     368          14 :     state_heading_ = uav_heading;
+     369             : 
+     370          14 :     speed_x_         = uav_state.velocity.linear.x;
+     371          14 :     speed_y_         = uav_state.velocity.linear.y;
+     372          14 :     current_heading_ = atan2(speed_y_, speed_x_);
+     373             : 
+     374          14 :     current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     375             : 
+     376          14 :     current_vertical_speed_     = fabs(uav_state.velocity.linear.z);
+     377          14 :     current_vertical_direction_ = uav_state.velocity.linear.z > 0 ? +1 : -1;
+     378             : 
+     379          14 :     current_horizontal_acceleration_ = 0;
+     380          14 :     current_vertical_acceleration_   = 0;
+     381             : 
+     382          14 :     goal_heading_ = uav_heading;
+     383             : 
+     384          14 :     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          14 :     std::scoped_lock lock(mutex_state_);
+     395             : 
+     396          14 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     397          14 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     398          14 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     399          14 :     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          14 :     std::scoped_lock lock(mutex_state_);
+     410             : 
+     411          14 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     412          14 :     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          14 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     421             : 
+     422          14 :     goal_x_ = state_x_ + stop_dist_x;
+     423          14 :     goal_y_ = state_y_ + stop_dist_y;
+     424          14 :     goal_z_ = state_z_ + vertical_stop_dist;
+     425             :   }
+     426             : 
+     427          14 :   landing_        = false;
+     428          14 :   taking_off_     = false;
+     429          14 :   is_active_      = true;
+     430          14 :   have_goal_      = false;
+     431          14 :   cause_failsafe_ = false;
+     432             : 
+     433          14 :   timer_main_.start();
+     434             : 
+     435             :   {
+     436          28 :     std::scoped_lock lock(mutex_goal_);
+     437             : 
+     438          14 :     ROS_INFO("[LandoffTracker]: stopping goal: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     439             :   }
+     440             : 
+     441          14 :   changeState(STOP_MOTION_STATE);
+     442             : 
+     443          14 :   ss << "activated";
+     444          14 :   ROS_INFO_STREAM("[LandoffTracker]: " << ss.str());
+     445             : 
+     446          14 :   return std::tuple(true, ss.str());
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : /* //{ deactivate() */
+     452             : 
+     453          20 : void LandoffTracker::deactivate(void) {
+     454             : 
+     455          20 :   is_active_                = false;
+     456          20 :   landing_                  = false;
+     457          20 :   taking_off_               = false;
+     458          20 :   current_state_vertical_   = IDLE_STATE;
+     459          20 :   current_state_horizontal_ = IDLE_STATE;
+     460             : 
+     461          20 :   timer_main_.stop();
+     462             : 
+     463          20 :   ROS_INFO("[LandoffTracker]: deactivated");
+     464          20 : }
+     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       39318 : 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      117954 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     483      117954 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     484             : 
+     485             :   {
+     486       39318 :     std::scoped_lock lock(mutex_uav_state_);
+     487             : 
+     488       39318 :     uav_state_ = uav_state;
+     489             : 
+     490       39318 :     got_uav_state_ = true;
+     491             :   }
+     492             : 
+     493             :   // up to this part the update() method is evaluated even when the tracker is not active
+     494       39318 :   if (!is_active_ || cause_failsafe_) {
+     495       32636 :     return {};
+     496             :   }
+     497             : 
+     498        6682 :   position_output_.header.stamp    = ros::Time::now();
+     499        6682 :   position_output_.header.frame_id = uav_state_.header.frame_id;
+     500             : 
+     501             :   {
+     502        6682 :     std::scoped_lock lock(mutex_state_);
+     503             : 
+     504        6682 :     position_output_.position.x = state_x_;
+     505        6682 :     position_output_.position.y = state_y_;
+     506        6682 :     position_output_.position.z = state_z_;
+     507        6682 :     position_output_.heading    = state_heading_;
+     508             : 
+     509        6682 :     position_output_.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     510        6682 :     position_output_.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     511        6682 :     position_output_.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     512        6682 :     position_output_.heading_rate = speed_heading_;
+     513             : 
+     514        6682 :     position_output_.use_position_vertical   = 1;
+     515        6682 :     position_output_.use_position_horizontal = 1;
+     516        6682 :     position_output_.use_heading             = 1;
+     517        6682 :     position_output_.use_heading_rate        = 1;
+     518        6682 :     position_output_.use_velocity_vertical   = 1;
+     519        6682 :     position_output_.use_velocity_horizontal = 1;
+     520             :   }
+     521             : 
+     522             :   {
+     523       13364 :     std::scoped_lock lock(mutex_last_control_output_);
+     524             : 
+     525        6682 :     last_control_output_ = last_control_output;
+     526             :   }
+     527             : 
+     528        6682 :   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        6682 :     position_output_.disable_position_gains = false;
+     532             :   }
+     533             : 
+     534        6682 :   if (taking_off_) {
+     535        2581 :     position_output_.disable_antiwindups = true;
+     536             :   } else {
+     537        4101 :     position_output_.disable_antiwindups = false;
+     538             :   }
+     539             : 
+     540        6682 :   return {position_output_};
+     541             : }
+     542             : 
+     543             : //}
+     544             : 
+     545             : /* //{ getStatus() */
+     546             : 
+     547         716 : const mrs_msgs::TrackerStatus LandoffTracker::getStatus() {
+     548             : 
+     549         716 :   mrs_msgs::TrackerStatus tracker_status;
+     550             : 
+     551         716 :   tracker_status.active            = is_active_;
+     552         716 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     553             : 
+     554         716 :   bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
+     555         716 :   bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     556             : 
+     557         716 :   tracker_status.have_goal = landing_ || taking_off_ || !(hovering || idling);
+     558             : 
+     559         716 :   tracker_status.tracking_trajectory = false;
+     560             : 
+     561         716 :   return tracker_status;
+     562             : }
+     563             : 
+     564             : //}
+     565             : 
+     566             : /* //{ enableCallbacks() */
+     567             : 
+     568           2 : const std_srvs::SetBoolResponse::ConstPtr LandoffTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+     569             : 
+     570           4 :   std_srvs::SetBoolResponse res;
+     571           4 :   std::stringstream         ss;
+     572             : 
+     573           2 :   if (cmd->data != callbacks_enabled_) {
+     574             : 
+     575           0 :     callbacks_enabled_ = cmd->data;
+     576             : 
+     577           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     578           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     579             : 
+     580             :   } else {
+     581             : 
+     582           2 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     583           2 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     584             :   }
+     585             : 
+     586           2 :   res.message = ss.str();
+     587           2 :   res.success = true;
+     588             : 
+     589           4 :   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         123 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LandoffTracker::setConstraints([
+     755             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd) {
+     756             : 
+     757             : 
+     758         123 :   mrs_lib::set_mutexed(mutex_constraints_, cmd->constraints, constraints_);
+     759             : 
+     760         123 :   ROS_INFO("[LandoffTracker]: updating constraints");
+     761             : 
+     762         246 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     763         123 :   res.success = true;
+     764         123 :   res.message = "constraints updated";
+     765             : 
+     766         246 :   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           1 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LandoffTracker::setTrajectoryReference([
+     792             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+     793           1 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     794             : }
+     795             : 
+     796             : //}
+     797             : 
+     798             : // | ----------------- state machine routines ----------------- |
+     799             : 
+     800             : /* //{ changeStateHorizontal() */
+     801             : 
+     802          54 : void LandoffTracker::changeStateHorizontal(States_t new_state) {
+     803             : 
+     804          54 :   previous_state_horizontal_ = current_state_horizontal_;
+     805          54 :   current_state_horizontal_  = new_state;
+     806             : 
+     807          54 :   switch (current_state_horizontal_) {
+     808             : 
+     809          17 :     case STOPPING_STATE: {
+     810             : 
+     811          34 :       std::scoped_lock lock(mutex_state_);
+     812          17 :       current_horizontal_speed_ = 0;
+     813             : 
+     814          17 :       break;
+     815             :     };
+     816             : 
+     817          37 :     default: {
+     818             : 
+     819          37 :       break;
+     820             :     }
+     821             :   }
+     822             : 
+     823          54 :   ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     824          54 : }
+     825             : 
+     826             : //}
+     827             : 
+     828             : /* //{ changeStateVertical() */
+     829             : 
+     830          66 : void LandoffTracker::changeStateVertical(States_t new_state) {
+     831             : 
+     832          66 :   previous_state_vertical_ = current_state_vertical_;
+     833          66 :   current_state_vertical_  = new_state;
+     834             : 
+     835          66 :   switch (current_state_vertical_) {
+     836             : 
+     837          12 :     case HOVER_STATE: {
+     838          12 :       taking_off_ = false;
+     839          12 :       break;
+     840             :     }
+     841             : 
+     842          54 :     default: {
+     843          54 :       break;
+     844             :     }
+     845             :   }
+     846             : 
+     847          66 :   ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     848          66 : }
+     849             : 
+     850             : //}
+     851             : 
+     852             : /* //{ changeState() */
+     853             : 
+     854          43 : void LandoffTracker::changeState(States_t new_state) {
+     855             : 
+     856          43 :   changeStateVertical(new_state);
+     857          43 :   changeStateHorizontal(new_state);
+     858          43 : }
+     859             : 
+     860             : //}
+     861             : 
+     862             : // | --------------------- motion routines -------------------- |
+     863             : 
+     864             : /* //{ stopHorizontalMotion() */
+     865             : 
+     866         132 : void LandoffTracker::stopHorizontalMotion(void) {
+     867             : 
+     868             :   {
+     869         264 :     std::scoped_lock lock(mutex_state_);
+     870             : 
+     871         132 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     872             : 
+     873         132 :     if (current_horizontal_speed_ < 0) {
+     874           9 :       current_horizontal_speed_        = 0;
+     875           9 :       current_horizontal_acceleration_ = 0;
+     876             :     } else {
+     877         123 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     878             :     }
+     879             :   }
+     880         132 : }
+     881             : 
+     882             : //}
+     883             : 
+     884             : /* //{ stopVerticalMotion() */
+     885             : 
+     886         132 : void LandoffTracker::stopVerticalMotion(void) {
+     887             : 
+     888             :   {
+     889         264 :     std::scoped_lock lock(mutex_state_);
+     890             : 
+     891         132 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     892             : 
+     893         132 :     if (current_vertical_speed_ < 0) {
+     894          98 :       current_vertical_speed_        = 0;
+     895          98 :       current_vertical_acceleration_ = 0;
+     896             :     } else {
+     897          34 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     898             :     }
+     899             :   }
+     900         132 : }
+     901             : 
+     902             : //}
+     903             : 
+     904             : /* //{ accelerateVertical() */
+     905             : 
+     906        5328 : void LandoffTracker::accelerateVertical(void) {
+     907             : 
+     908             :   // copy member variables
+     909        5328 :   auto [current_vertical_speed, state_z] = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_, state_z_);
+     910        5328 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     911        5328 :   auto constraints                       = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     912             : 
+     913             :   double used_acceleration;
+     914             :   double used_speed;
+     915             : 
+     916        5328 :   if (taking_off_) {
+     917             : 
+     918        1344 :     used_speed        = _takeoff_speed_;
+     919        1344 :     used_acceleration = _takeoff_acceleration_;
+     920             : 
+     921        1344 :     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        1344 :     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        3984 :   } else if (landing_) {
+     932             : 
+     933        3984 :     if (elanding_) {
+     934             : 
+     935        2295 :       used_speed        = _elanding_speed_;
+     936        2295 :       used_acceleration = _elanding_acceleration_;
+     937             : 
+     938             :     } else {
+     939             : 
+     940        1689 :       used_speed        = _landing_speed_;
+     941        1689 :       used_acceleration = _landing_acceleration_;
+     942             : 
+     943        1689 :       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        1689 :       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        5328 :   double tar_z = goal_z - state_z;
+     963             : 
+     964             :   // set the right vertical direction
+     965             :   {
+     966        5328 :     std::scoped_lock lock(mutex_state_);
+     967             : 
+     968        5328 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+     969             :   }
+     970             : 
+     971        5328 :   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        5328 :   double vertical_t_stop    = current_vertical_speed / used_acceleration;
+     975        5328 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+     976        5328 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+     977             : 
+     978             :   {
+     979       10656 :     std::scoped_lock lock(mutex_state_);
+     980             : 
+     981        5328 :     current_vertical_speed_ += used_acceleration * _tracker_dt_;
+     982             : 
+     983        5328 :     if (current_vertical_speed_ >= used_speed) {
+     984        1285 :       current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     985        1285 :       current_vertical_acceleration_ = 0;
+     986             :     } else {
+     987        4043 :       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        5328 :   if (!elanding_ && !landing_) {
+     997        1344 :     if (fabs(state_z + stop_dist_z - goal_z) < (2 * (used_speed * _tracker_dt_))) {
+     998             : 
+     999             :       {
+    1000           6 :         std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002           6 :         current_vertical_acceleration_ = 0;
+    1003             :       }
+    1004             : 
+    1005           6 :       changeStateVertical(DECELERATING_STATE);
+    1006             :     }
+    1007             :   }
+    1008        5328 : }
+    1009             : 
+    1010             : //}
+    1011             : 
+    1012             : /* //{ decelerateVertical() */
+    1013             : 
+    1014        1344 : void LandoffTracker::decelerateVertical(void) {
+    1015             : 
+    1016             :   double used_acceleration;
+    1017             : 
+    1018        1344 :   if (taking_off_) {
+    1019             : 
+    1020        1344 :     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        2688 :     std::scoped_lock lock(mutex_state_);
+    1039             : 
+    1040        1344 :     current_vertical_speed_ -= used_acceleration * _tracker_dt_;
+    1041             : 
+    1042        1344 :     if (current_vertical_speed_ < 0) {
+    1043           6 :       current_vertical_speed_ = 0;
+    1044             :     } else {
+    1045        1338 :       current_vertical_acceleration_ = -used_acceleration;
+    1046             :     }
+    1047             :   }
+    1048             : 
+    1049        1344 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1050             : 
+    1051        1344 :   if (current_vertical_speed == 0) {
+    1052             : 
+    1053             :     {
+    1054           6 :       std::scoped_lock lock(mutex_state_);
+    1055             : 
+    1056           6 :       current_vertical_acceleration_ = 0;
+    1057             :     }
+    1058             : 
+    1059           6 :     changeStateVertical(STOPPING_STATE);
+    1060             :   }
+    1061        1344 : }
+    1062             : 
+    1063             : //}
+    1064             : 
+    1065             : /* //{ stopHorizontal() */
+    1066             : 
+    1067        6672 : void LandoffTracker::stopHorizontal(void) {
+    1068             : 
+    1069             :   {
+    1070        6672 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1071             : 
+    1072        6672 :     double new_state_x = 0.95 * state_x_ + 0.05 * goal_x_;
+    1073        6672 :     double new_state_y = 0.95 * state_y_ + 0.05 * goal_y_;
+    1074             : 
+    1075        6672 :     double dist_x = new_state_x - state_x_;
+    1076        6672 :     double dist_y = new_state_y - state_y_;
+    1077             : 
+    1078        6672 :     double dt = 1.0 / _main_timer_rate_;
+    1079             : 
+    1080        6672 :     if (std::abs(dist_x / dt) > 1.0) {
+    1081           0 :       dist_x = mrs_lib::signum(dist_x) * (1.0 * dt);
+    1082             :     }
+    1083             : 
+    1084        6672 :     if (std::abs(dist_y / dt) > 1.0) {
+    1085           0 :       dist_y = mrs_lib::signum(dist_y) * (1.0 * dt);
+    1086             :     }
+    1087             : 
+    1088        6672 :     state_x_ += dist_x;
+    1089        6672 :     state_y_ += dist_y;
+    1090             : 
+    1091        6672 :     current_horizontal_acceleration_ = 0;
+    1092             :   }
+    1093        6672 : }
+    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        7160 : void LandoffTracker::timerMain(const ros::TimerEvent& event) {
+    1127             : 
+    1128        7160 :   std::scoped_lock lock(mutex_main_timer_);
+    1129             : 
+    1130        7160 :   if (!is_active_) {
+    1131           0 :     return;
+    1132             :   }
+    1133             : 
+    1134             :   // copy member variables
+    1135       14320 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1136        7160 :   auto [state_x, state_y, state_z, current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] = mrs_lib::get_mutexed(
+    1137        7160 :       mutex_state_, state_x_, state_y_, state_z_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+    1138        7160 :   auto [goal_x, goal_y, goal_z] = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1139       14320 :   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        7160 :   uav_x = uav_state.pose.position.x;
+    1143        7160 :   uav_y = uav_state.pose.position.y;
+    1144        7160 :   uav_z = uav_state.pose.position.z;
+    1145             : 
+    1146       21480 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _main_timer_rate_, 0.002, event);
+    1147       21480 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1148             : 
+    1149        7160 :   bool takeoff_saturated = false;
+    1150             : 
+    1151        7160 :   if (taking_off_) {
+    1152             : 
+    1153             :     // calculate the vector
+    1154        2808 :     double err_x      = uav_x - state_x;
+    1155        2808 :     double err_y      = uav_y - state_y;
+    1156        2808 :     double err_z      = uav_z - state_z;
+    1157        2808 :     double error_size = sqrt(pow(err_x, 2) + pow(err_y, 2) + pow(err_z, 2));
+    1158             : 
+    1159        2808 :     if (error_size > _max_position_difference_) {
+    1160             : 
+    1161             :       // calculate the potential next step
+    1162           0 :       double future_state_x = state_x + cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1163           0 :       double future_state_y = state_y + sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1164           0 :       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           0 :       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           0 :         takeoff_saturated = true;
+    1172             : 
+    1173           0 :         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        2808 :     if (last_control_output.diagnostics.ramping_up) {
+    1181             : 
+    1182         120 :       ROS_INFO_THROTTLE(1.0, "[LandoffTracker]: waiting for the controller to rampup");
+    1183         120 :       takeoff_saturated = true;
+    1184             :     }
+    1185             :   }
+    1186             : 
+    1187        7160 :   if (!takeoff_saturated) {
+    1188             : 
+    1189        7040 :     switch (current_state_horizontal_) {
+    1190             : 
+    1191         132 :       case STOP_MOTION_STATE: {
+    1192             : 
+    1193         132 :         stopHorizontalMotion();
+    1194         132 :         break;
+    1195             :       }
+    1196             : 
+    1197        6672 :       case STOPPING_STATE: {
+    1198             : 
+    1199        6672 :         stopHorizontal();
+    1200        6672 :         break;
+    1201             :       }
+    1202             : 
+    1203         236 :       default: {
+    1204             : 
+    1205         236 :         break;
+    1206             :       }
+    1207             :     }
+    1208             : 
+    1209        7040 :     switch (current_state_vertical_) {
+    1210             : 
+    1211         132 :       case STOP_MOTION_STATE: {
+    1212             : 
+    1213         132 :         stopVerticalMotion();
+    1214         132 :         break;
+    1215             :       }
+    1216             : 
+    1217        5328 :       case ACCELERATING_STATE: {
+    1218             : 
+    1219        5328 :         accelerateVertical();
+    1220        5328 :         break;
+    1221             :       }
+    1222             : 
+    1223        1344 :       case DECELERATING_STATE: {
+    1224             : 
+    1225        1344 :         decelerateVertical();
+    1226        1344 :         break;
+    1227             :       }
+    1228             : 
+    1229           0 :       case STOPPING_STATE: {
+    1230             : 
+    1231           0 :         stopVertical();
+    1232           0 :         break;
+    1233             :       }
+    1234             : 
+    1235         236 :       default: {
+    1236             : 
+    1237         236 :         break;
+    1238             :       }
+    1239             :     }
+    1240             :   }
+    1241             : 
+    1242        7160 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1243         138 :     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          17 :       if (have_goal_) {
+    1250             : 
+    1251          11 :         changeStateVertical(ACCELERATING_STATE);
+    1252          11 :         changeStateHorizontal(STOPPING_STATE);
+    1253             : 
+    1254             :       } else {
+    1255             : 
+    1256           6 :         changeState(STOPPING_STATE);
+    1257             : 
+    1258             :         {
+    1259           6 :           std::scoped_lock lock(mutex_state_);
+    1260             : 
+    1261           6 :           current_horizontal_speed_ = 0;
+    1262           6 :           current_vertical_speed_   = 0;
+    1263             :         }
+    1264             :       }
+    1265             :     }
+    1266             :   }
+    1267             : 
+    1268        7160 :   if (current_state_vertical_ == STOPPING_STATE && current_state_horizontal_ == STOPPING_STATE) {
+    1269             : 
+    1270          12 :     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          12 :     } 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          12 :         std::scoped_lock lock(mutex_state_);
+    1283             : 
+    1284          12 :         if (!taking_off_) {
+    1285           6 :           state_x_ = goal_x;
+    1286           6 :           state_y_ = goal_y;
+    1287           6 :           state_z_ = goal_z;
+    1288             :         }
+    1289             : 
+    1290          12 :         current_horizontal_speed_ = 0;
+    1291          12 :         current_vertical_speed_   = 0;
+    1292             :       }
+    1293             : 
+    1294          12 :       changeState(HOVER_STATE);
+    1295             : 
+    1296          12 :       have_goal_ = false;
+    1297             :     }
+    1298             :   }
+    1299             : 
+    1300        7160 :   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        7160 :   if (!takeoff_saturated) {
+    1318             :     {
+    1319        7040 :       std::scoped_lock lock(mutex_state_);
+    1320             : 
+    1321        7040 :       state_x_ += cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1322        7040 :       state_y_ += sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1323        7040 :       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       14320 :     std::scoped_lock lock(mutex_state_);
+    1334             : 
+    1335             :     double current_heading_rate;
+    1336             : 
+    1337        7160 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1338           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1339             :     else
+    1340        7160 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1341             : 
+    1342        7160 :     if (current_heading_rate > _heading_rate_) {
+    1343           0 :       current_heading_rate = _heading_rate_;
+    1344        7160 :     } 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        7160 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1350             : 
+    1351        7160 :     if (state_heading_ > M_PI) {
+    1352           0 :       state_heading_ -= 2 * M_PI;
+    1353        7160 :     } else if (state_heading_ < -M_PI) {
+    1354           0 :       state_heading_ += 2 * M_PI;
+    1355             :     }
+    1356             : 
+    1357        7160 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1358        7160 :       state_heading_ = goal_heading_;
+    1359             :     }
+    1360             :   }
+    1361             : 
+    1362             :   // --------------------------------------------------------------
+    1363             :   // |                      landing setpoint                      |
+    1364             :   // --------------------------------------------------------------
+    1365             : 
+    1366        7160 :   if (landing_) {
+    1367             :     {
+    1368        4110 :       std::scoped_lock lock(mutex_goal_);
+    1369             : 
+    1370        4110 :       goal_z_ = uav_z + _landing_reference_;
+    1371             :     }
+    1372             :   }
+    1373             : }
+    1374             : 
+    1375             : //}
+    1376             : 
+    1377             : // | ------------------------ callbacks ----------------------- |
+    1378             : 
+    1379             : /* //{ callbackTakeoff() */
+    1380             : 
+    1381           6 : bool LandoffTracker::callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    1382             : 
+    1383          12 :   std::stringstream ss;
+    1384             : 
+    1385             :   // copy member variables
+    1386          12 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1387             : 
+    1388           6 :   double uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1389             : 
+    1390             :   double uav_x, uav_y, uav_z;
+    1391           6 :   uav_x = uav_state.pose.position.x;
+    1392           6 :   uav_y = uav_state.pose.position.y;
+    1393           6 :   uav_z = uav_state.pose.position.z;
+    1394             : 
+    1395           6 :   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           6 :   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           6 :   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          12 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+    1422             : 
+    1423           6 :     state_x_ = uav_x;
+    1424           6 :     goal_x_  = uav_x;
+    1425             : 
+    1426           6 :     state_y_ = uav_y;
+    1427           6 :     goal_y_  = uav_y;
+    1428             : 
+    1429           6 :     state_z_ = uav_z;
+    1430           6 :     goal_z_  = uav_z + req.goal;
+    1431             : 
+    1432           6 :     state_heading_ = uav_heading;
+    1433           6 :     goal_heading_  = uav_heading;
+    1434             : 
+    1435           6 :     speed_x_                = 0;
+    1436           6 :     speed_y_                = 0;
+    1437           6 :     current_vertical_speed_ = 0;
+    1438             : 
+    1439           6 :     have_goal_ = true;
+    1440             :   }
+    1441             : 
+    1442           6 :   ROS_INFO("[LandoffTracker]: taking off");
+    1443             : 
+    1444           6 :   taking_off_ = true;
+    1445           6 :   landing_    = false;
+    1446           6 :   elanding_   = false;
+    1447             : 
+    1448           6 :   res.success = true;
+    1449           6 :   res.message = "taking off";
+    1450             : 
+    1451           6 :   changeState(STOP_MOTION_STATE);
+    1452             : 
+    1453           6 :   return true;
+    1454             : }
+    1455             : 
+    1456             : //}
+    1457             : 
+    1458             : /* //{ callbackLand() */
+    1459             : 
+    1460           2 : bool LandoffTracker::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1461             : 
+    1462           4 :   std::scoped_lock lock(mutex_main_timer_);
+    1463             : 
+    1464           4 :   std::stringstream ss;
+    1465             : 
+    1466             :   // copy member variables
+    1467           4 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1468             : 
+    1469           2 :   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           2 :     std::scoped_lock lock(mutex_goal_);
+    1479             : 
+    1480           2 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1481             :   }
+    1482             : 
+    1483           2 :   ROS_INFO("[LandoffTracker]: landing");
+    1484             : 
+    1485           2 :   landing_    = true;
+    1486           2 :   elanding_   = false;
+    1487           2 :   taking_off_ = false;
+    1488           2 :   have_goal_  = true;
+    1489             : 
+    1490           2 :   res.success = true;
+    1491           2 :   res.message = "landing";
+    1492             : 
+    1493           2 :   changeState(STOP_MOTION_STATE);
+    1494             : 
+    1495           2 :   return true;
+    1496             : }
+    1497             : 
+    1498             : //}
+    1499             : 
+    1500             : /* //{ callbackELand() */
+    1501             : 
+    1502           3 : bool LandoffTracker::callbackELand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1503             : 
+    1504           6 :   std::scoped_lock lock(mutex_main_timer_);
+    1505             : 
+    1506           6 :   std::stringstream ss;
+    1507             : 
+    1508             :   // copy member variables
+    1509           6 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1510             : 
+    1511           3 :   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           3 :     std::scoped_lock lock(mutex_goal_);
+    1526             : 
+    1527           3 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1528             :   }
+    1529             : 
+    1530           3 :   ROS_WARN("[LandoffTracker]: emergency landing");
+    1531             : 
+    1532           3 :   landing_    = true;
+    1533           3 :   elanding_   = true;
+    1534           3 :   taking_off_ = false;
+    1535           3 :   have_goal_  = true;
+    1536             : 
+    1537           3 :   res.success = true;
+    1538           3 :   res.message = "elanding";
+    1539             : 
+    1540           3 :   changeState(STOP_MOTION_STATE);
+    1541             : 
+    1542           3 :   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          42 : 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..fa8087ee399268ccfcd840f4ed8e08c9845212b0 GIT binary patch literal 4783 zcmV;g5>V}lP)qz4wE;;L<#dr}lsEooyb`PJ|O zK}EC|%`seeW%{9+p2{PZ4HQ;APeq-y9n-Jw0X<{Yr!-!=*Nm#pQXf$Dq%~64-D3u^ z^vXcRG=pM)t{-guV{0ng)UpjSdz?st{p30KCHF9v!j6S$>O|9!bi>;y`~=?#+qhVR z=80Z4%oxZOj&F-2H^mOl0>6+gd}bV_2~|%xR{>SWGC`39HW(Lz!}$?5bXs87fTmHk z2mOY42QQ1jMXWs}MN7ePGbgIHVB{rdfIN4a)y6sIog*IgmIa)+zzfL$&Zdp6J>wc@ z$Vv!u>jxFga~GRRasgEw0rOuMHpf@S0nUQym|JzGm`*7JXMXSLs;7n5bBZdOdrWQN zcL5+z7E$UVNbIZ6a8_0y$m?+5Erm7Q->g?UC*Y0_Yw^r#d$ha>OZ91v9vj#_f&U83Jc6aA5=I z1W>?C7?Mg5jADSPijm%S=s@6EdX^bL-7O5@oB>w0G22QuMOJ7p0kmA@KM7Q zxAd}B;kq!8O+y=t(Hi$h&r;)R1v3-OTg?J@ibZe6A!hwoLY%k>#U>8e)0_am$OhDk z(FVV}WmG($+Y^`FzS!WO9&1pZiNdcMl2~1>VLj`LfV%_k1dv$_#SO6@)00trw~Qju z*v7P26#$tO*~JW+d2wcbGV}XXH)wzyLsdtg*Y-?E$19VB zBHgqt1ioHhs2taS$JhD(r^`zh4SfIgeC5mIRYF*xxC<8_V4C3+($t07A^-~dT3a0R zbB~Fw(!Nd>L|p*;fgMB383dzYy?qYL+cz)YTo@`ktsO z_ZvR$@5Vx-k86yxj`!yFtyaJMKm(AddJNE8fYitkm}XmbKDk;7j7TX}Q>d;v2?{aZj&x{sRV#gaI0&Fn4{*lC(^&ka?xxq%EQKS0d_Ekkd1Ys2NH6M|o0F zK)Rv}^?1+vDJ?)+(FXLjb9?rLYP+f}13aLnm=C%DAcLZjP!`B6FpN_Gaw&e$vnRY1 z@0z$5Lo7w;!*y`jRdb6SLu~_Y=9~3Z`#zy4IpCclaYHlMx@^1Hx9P#{jT8&$w6F-Y zO1meOH`)+O1DOR-9I|Pb3re#UmtFNh#qD-U4G2iNz*Hg}#*9fZH|<{!W{^t|Jo_XguQZT|mng|Gt?soAdc4< zw!#4M6lj1uv^JHiVZ}&LvHsPPnM#hq+E7y#c&owHZG2IQUc?e@uT6^0?F~E7^)|hj z5@&}&Z6ZLJKC)I7Z3%N;%g49b=Z4Q@XdFX!*cogEu zLlPzbD^@^Z^ZZN~sL z_N&ia=zWR<{yyBRvyP&1`M=V+`v92dyTB{Fczfa0Nm5krV~U6E>jBP3 z?__t!%lSgXhIADye+P}a-Z|5JF$O3?q&*GD26FrQ4vFM&p25n4<&P(%pgSD&{rN!g z*mraI6oVgdIE}CrC)66pHCsH2q89XV5R`fVb+5<3LF>ZPIZ>(kV~{5WON#FQ?VGvM zzFWa};wU*%;<=$5JA_^TwO_!wH;=%YUUyt8k&S$buJp zV?uuGnKQbLMADb8hOuAhfUbE7prnnN`|(__-eqHP>N*lglR_DfX5TEbG+BAOp3E(x29n1brKMRj+(z zc(!7|W4{Tv(B`RK8Ec!hg*Q8=*=x;}vOR(6skJ@f4I3$7#`LtDDW zMDeoBhWgYp#S1 zk1W;=kFoB7J@@xzizs3Tcu$`N2-TLn9xnvymeD}op);Bydh03jxMp|ZJw_oMb3I~1 z^q7e!`TLC-JY>)f^VMS}HDp4?O%54K!;`@3F@uK;q{!g;Z_Jd+fH5PzrP}Fu)Z6-F zceD|f7`Qz>#Vk;El|D>X%65$fcIXr%9``|^;kUwKH_)i7o1(5|k0}N^E#+wZ?^OZjPAz zNN8@siNhUJU3LFsl_WDcu1mvECdc$J^hRIpoF#`L&_E9?48se^(p;Gwi^K5E6KZ2> z7@mA)^u{8WneHw?_fx)h!k+w7VrZBV&c)aXVMTTSz@8X^ zvcZL_4xfa8$`4soWX#H#gWBPWJuK?)GJ!At_}Xl4G*fq+*^iVq<4Yo7z2MsMHHQ|a6e;{0GCRe%Nywb!&;dAlu2e2hNP-#!GR6kwb zjE>1%1vtBl#v_W?-)FPXyxz`KN!_Pn`xCnHvN5R%f$`u=N{49BW52^fhldP?51P!m@l`KfM9W(4jvmleVl>ul4qhUQ_~wRio8L@`7w0P*nAs5U@`PAfKYY(p`~F%nI67K+)d!n=`tz zVeD?oPj2JWtXxYegKQwb3z??*ciP8(r-q`f4D$Lk$%UKDss|nOKj07mFPbQ#TeO<4 zywMcjgn(n80P-nNxCcJ1Z^N=Ky;#=J$o-IuqkIhuwbbOl}f|M{jB8Y$^_OBU-#Ot`uKP+PX zEZ2;U7cti?AeWim!!RB*?KcdJ9W!Z3eO!CCf)Lj~t{H9o@mwQcCX?7uwNGD0s%!LMN*5$v0c9Vfyedr<4F`Qu%YTl_mrWyjTx@FZR9EW3hU0K zHOP8~bJb4-ydI*M1ad3@o9O{KcA;~$qtp44X9MEI-S-b`bMSjXCSbVvp-vO2NA&2Q z>IL=eg6nhm96dO5u_h!>vDag_f;J{e@+q$1@SYCF?cRoi_xH(^wn|uBh&!$Nb|BXP zFh}9ayN)bgv1fo|6UqB5rJP+TEX03AU!^fNxF*5Z%Gn$IT7Us| zx6a3h&vS{?OusWr9{AZTq$q2nUA(CkRs#6B^m%+PrpJ{VXf^DVX4}G+Vkge+`WB1+W-D%fw!y+N%v5CR;mo zz|T*^eU1%=rmhRcUo#EQ6WT57KtG_^VOa`N50)&|cw(ccx_<*EiQIE;HXHx|002ov JPDHLkV1m*RHWUB= literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..de28a5809f --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index-detail-sort-f.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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
<unnamed>68.1 %323 / 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..a5226de39c --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
<unnamed>68.1 %323 / 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..fa8f9e6a90 --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 47463.3 %19 / 30
<unnamed>68.1 %323 / 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..6a06c093fb --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 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..5ff87acf8f --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 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..fee07688d7 --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
68.1%68.1%
+ 		68.1 %323 / 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..712456bd1b --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::line_tracker::LineTracker::deactivate()0
mrs_uav_trackers::line_tracker::LineTracker::resetStatic()0
mrs_uav_trackers::line_tracker::LineTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::line_tracker::LineTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_trackers::line_tracker::LineTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)1
mrs_uav_trackers::line_tracker::LineTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::line_tracker::LineTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)1
mrs_uav_trackers::line_tracker::LineTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::line_tracker::LineTracker::changeState(mrs_uav_trackers::line_tracker::States_t)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::stopVerticalMotion()29
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontalMotion()29
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontal()52
mrs_uav_trackers::line_tracker::LineTracker::decelerateVertical()100
mrs_uav_trackers::line_tracker::LineTracker::decelerateHorizontal()100
mrs_uav_trackers::line_tracker::LineTracker::getStatus()208
mrs_uav_trackers::line_tracker::LineTracker::accelerateVertical()354
mrs_uav_trackers::line_tracker::LineTracker::stopVertical()1496
mrs_uav_trackers::line_tracker::LineTracker::accelerateHorizontal()1798
mrs_uav_trackers::line_tracker::LineTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)2036
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2602
+
+
+ + + +
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..1c6cc97470 --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions: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()1496
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()52
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()354
mrs_uav_trackers::line_tracker::LineTracker::decelerateVertical()100
mrs_uav_trackers::line_tracker::LineTracker::stopVerticalMotion()29
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()1798
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()29
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&)2036
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()208
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2602
+
+
+ + + +
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..29f7efbb4c --- /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:32347468.1 %
Date:2023-12-06 07:59:45Functions: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        2036 : 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        6108 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     519        6108 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LineTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     520             : 
+     521             :   {
+     522        2036 :     std::scoped_lock lock(mutex_uav_state_);
+     523             : 
+     524        2036 :     uav_state_ = uav_state;
+     525        2036 :     uav_x_     = uav_state_.pose.position.x;
+     526        2036 :     uav_y_     = uav_state_.pose.position.y;
+     527        2036 :     uav_z_     = uav_state_.pose.position.z;
+     528             : 
+     529        2036 :     got_uav_state_ = true;
+     530             :   }
+     531             : 
+     532             :   // up to this part the update() method is evaluated even when the tracker is not active
+     533        2036 :   if (!is_active_) {
+     534         101 :     return {};
+     535             :   }
+     536             : 
+     537        3870 :   mrs_msgs::TrackerCommand tracker_cmd;
+     538             : 
+     539        1935 :   tracker_cmd.header.stamp    = ros::Time::now();
+     540        1935 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     541             : 
+     542             :   {
+     543        1935 :     std::scoped_lock lock(mutex_state_);
+     544             : 
+     545        1935 :     tracker_cmd.position.x = state_x_;
+     546        1935 :     tracker_cmd.position.y = state_y_;
+     547        1935 :     tracker_cmd.position.z = state_z_;
+     548        1935 :     tracker_cmd.heading    = radians::wrap(state_heading_);
+     549             : 
+     550        1935 :     tracker_cmd.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     551        1935 :     tracker_cmd.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     552        1935 :     tracker_cmd.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     553        1935 :     tracker_cmd.heading_rate = speed_heading_;
+     554             : 
+     555        1935 :     tracker_cmd.acceleration.x = 0;
+     556        1935 :     tracker_cmd.acceleration.y = 0;
+     557        1935 :     tracker_cmd.acceleration.z = current_vertical_direction_ * current_vertical_acceleration_;
+     558             : 
+     559        1935 :     tracker_cmd.use_position_vertical   = 1;
+     560        1935 :     tracker_cmd.use_position_horizontal = 1;
+     561        1935 :     tracker_cmd.use_heading             = 1;
+     562        1935 :     tracker_cmd.use_heading_rate        = 1;
+     563        1935 :     tracker_cmd.use_velocity_vertical   = 1;
+     564        1935 :     tracker_cmd.use_velocity_horizontal = 1;
+     565        1935 :     tracker_cmd.use_acceleration        = 1;
+     566             :   }
+     567             : 
+     568        1935 :   return {tracker_cmd};
+     569             : }
+     570             : 
+     571             : //}
+     572             : 
+     573             : /* //{ getStatus() */
+     574             : 
+     575         208 : const mrs_msgs::TrackerStatus LineTracker::getStatus() {
+     576             : 
+     577         208 :   mrs_msgs::TrackerStatus tracker_status;
+     578             : 
+     579         208 :   tracker_status.active            = is_active_;
+     580         208 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     581             : 
+     582         208 :   bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     583             : 
+     584         208 :   tracker_status.have_goal = !idling;
+     585             : 
+     586         208 :   tracker_status.tracking_trajectory = false;
+     587             : 
+     588         208 :   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          29 : void LineTracker::stopHorizontalMotion(void) {
+     901             : 
+     902             :   {
+     903          58 :     std::scoped_lock lock(mutex_state_);
+     904             : 
+     905          29 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     906             : 
+     907          29 :     if (current_horizontal_speed_ < 0) {
+     908          29 :       current_horizontal_speed_        = 0;
+     909          29 :       current_horizontal_acceleration_ = 0;
+     910             :     } else {
+     911           0 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     912             :     }
+     913             :   }
+     914          29 : }
+     915             : 
+     916             : //}
+     917             : 
+     918             : /* //{ stopVerticalMotion() */
+     919             : 
+     920          29 : void LineTracker::stopVerticalMotion(void) {
+     921             : 
+     922             :   {
+     923          58 :     std::scoped_lock lock(mutex_state_);
+     924             : 
+     925          29 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     926             : 
+     927          29 :     if (current_vertical_speed_ < 0) {
+     928           2 :       current_vertical_speed_        = 0;
+     929           2 :       current_vertical_acceleration_ = 0;
+     930             :     } else {
+     931          27 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     932             :     }
+     933             :   }
+     934          29 : }
+     935             : 
+     936             : //}
+     937             : 
+     938             : /* //{ accelerateHorizontal() */
+     939             : 
+     940        1798 : void LineTracker::accelerateHorizontal(void) {
+     941             : 
+     942             :   // copy member variables
+     943        1798 :   auto [goal_x, goal_y]                             = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_);
+     944        1798 :   auto [state_x, state_y, current_horizontal_speed] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, current_horizontal_speed_);
+     945             : 
+     946             :   {
+     947        1798 :     std::scoped_lock lock(mutex_state_);
+     948             : 
+     949        1798 :     current_heading_ = atan2(goal_y - state_y, goal_x - state_x);
+     950             :   }
+     951             : 
+     952        1798 :   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        1798 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     957        1798 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     958        1798 :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     959        1798 :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     960             : 
+     961             :   {
+     962        3596 :     std::scoped_lock lock(mutex_state_);
+     963             : 
+     964        1798 :     current_horizontal_speed_ += _horizontal_acceleration_ * _tracker_dt_;
+     965             : 
+     966        1798 :     if (current_horizontal_speed_ >= _horizontal_speed_) {
+     967        1699 :       current_horizontal_speed_        = _horizontal_speed_;
+     968        1699 :       current_horizontal_acceleration_ = 0;
+     969             :     } else {
+     970          99 :       current_horizontal_acceleration_ = _horizontal_acceleration_;
+     971             :     }
+     972             :   }
+     973             : 
+     974        1798 :   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        1798 : }
+     985             : 
+     986             : //}
+     987             : 
+     988             : /* //{ accelerateVertical() */
+     989             : 
+     990         354 : void LineTracker::accelerateVertical(void) {
+     991             : 
+     992         354 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     993         354 :   auto [state_z, current_vertical_speed] = mrs_lib::get_mutexed(mutex_state_, state_z_, current_vertical_speed_);
+     994             : 
+     995             :   // set the right heading
+     996         354 :   double tar_z = goal_z - state_z;
+     997             : 
+     998             :   // set the right vertical direction
+     999             :   {
+    1000         354 :     std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002         354 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+    1003             :   }
+    1004             : 
+    1005         354 :   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         354 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+    1009         354 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+    1010         354 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+    1011             : 
+    1012             :   {
+    1013         708 :     std::scoped_lock lock(mutex_state_);
+    1014             : 
+    1015         354 :     current_vertical_speed_ += _vertical_acceleration_ * _tracker_dt_;
+    1016             : 
+    1017         354 :     if (current_vertical_speed_ >= _vertical_speed_) {
+    1018         255 :       current_vertical_speed_        = _vertical_speed_;
+    1019         255 :       current_vertical_acceleration_ = 0;
+    1020             :     } else {
+    1021          99 :       current_vertical_acceleration_ = _vertical_acceleration_;
+    1022             :     }
+    1023             :   }
+    1024             : 
+    1025         354 :   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         354 : }
+    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          52 : void LineTracker::stopHorizontal(void) {
+    1100             : 
+    1101             :   {
+    1102          52 :     std::scoped_lock lock(mutex_state_);
+    1103             : 
+    1104          52 :     state_x_                         = 0.95 * state_x_ + 0.05 * goal_x_;
+    1105          52 :     state_y_                         = 0.95 * state_y_ + 0.05 * goal_y_;
+    1106          52 :     current_horizontal_acceleration_ = 0;
+    1107             :   }
+    1108          52 : }
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ stopVertical() */
+    1113             : 
+    1114        1496 : void LineTracker::stopVertical(void) {
+    1115             : 
+    1116             :   {
+    1117        1496 :     std::scoped_lock lock(mutex_state_);
+    1118             : 
+    1119        1496 :     state_z_                       = 0.95 * state_z_ + 0.05 * goal_z_;
+    1120        1496 :     current_vertical_acceleration_ = 0;
+    1121             :   }
+    1122        1496 : }
+    1123             : 
+    1124             : //}
+    1125             : 
+    1126             : // | ------------------------- timers ------------------------- |
+    1127             : 
+    1128             : /* //{ mainTimer() */
+    1129             : 
+    1130        2602 : void LineTracker::mainTimer(const ros::TimerEvent &event) {
+    1131             : 
+    1132        2602 :   if (!is_active_) {
+    1133         517 :     return;
+    1134             :   }
+    1135             : 
+    1136        6255 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _tracker_loop_rate_, 0.01, event);
+    1137        6255 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("LineTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1138             : 
+    1139        2085 :   auto [goal_x, goal_y, goal_z]    = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1140        2085 :   auto [state_x, state_y, state_z] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, state_z_);
+    1141             : 
+    1142        2085 :   switch (current_state_horizontal_) {
+    1143             : 
+    1144         106 :     case IDLE_STATE:
+    1145             : 
+    1146         106 :       break;
+    1147             : 
+    1148          29 :     case STOP_MOTION_STATE:
+    1149             : 
+    1150          29 :       stopHorizontalMotion();
+    1151             : 
+    1152          29 :       break;
+    1153             : 
+    1154        1798 :     case ACCELERATING_STATE:
+    1155             : 
+    1156        1798 :       accelerateHorizontal();
+    1157             : 
+    1158        1798 :       break;
+    1159             : 
+    1160         100 :     case DECELERATING_STATE:
+    1161             : 
+    1162         100 :       decelerateHorizontal();
+    1163             : 
+    1164         100 :       break;
+    1165             : 
+    1166          52 :     case STOPPING_STATE:
+    1167             : 
+    1168          52 :       stopHorizontal();
+    1169             : 
+    1170          52 :       break;
+    1171             :   }
+    1172             : 
+    1173        2085 :   switch (current_state_vertical_) {
+    1174             : 
+    1175         106 :     case IDLE_STATE:
+    1176             : 
+    1177         106 :       break;
+    1178             : 
+    1179          29 :     case STOP_MOTION_STATE:
+    1180             : 
+    1181          29 :       stopVerticalMotion();
+    1182             : 
+    1183          29 :       break;
+    1184             : 
+    1185         354 :     case ACCELERATING_STATE:
+    1186             : 
+    1187         354 :       accelerateVertical();
+    1188             : 
+    1189         354 :       break;
+    1190             : 
+    1191         100 :     case DECELERATING_STATE:
+    1192             : 
+    1193         100 :       decelerateVertical();
+    1194             : 
+    1195         100 :       break;
+    1196             : 
+    1197        1496 :     case STOPPING_STATE:
+    1198             : 
+    1199        1496 :       stopVertical();
+    1200             : 
+    1201        1496 :       break;
+    1202             :   }
+    1203             : 
+    1204        2085 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1205          29 :     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        2085 :   if (current_state_horizontal_ == STOPPING_STATE && current_state_vertical_ == STOPPING_STATE) {
+    1215          54 :     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        2085 :     std::scoped_lock lock(mutex_state_);
+    1233             : 
+    1234        2085 :     state_x_ += cos(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1235        2085 :     state_y_ += sin(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1236        2085 :     state_z_ += current_vertical_direction_ * current_vertical_speed_ * _tracker_dt_;
+    1237             :   }
+    1238             : 
+    1239             :   // --------------------------------------------------------------
+    1240             :   // |                        heading tracking                        |
+    1241             :   // --------------------------------------------------------------
+    1242             : 
+    1243             :   {
+    1244        4170 :     std::scoped_lock lock(mutex_state_);
+    1245             : 
+    1246             :     // compute the desired heading rate
+    1247             :     double current_heading_rate;
+    1248        2085 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1249           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1250             :     else
+    1251        2085 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1252             : 
+    1253        2085 :     if (current_heading_rate > _heading_rate_) {
+    1254         120 :       current_heading_rate = _heading_rate_;
+    1255        1965 :     } 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        2085 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1261             : 
+    1262        2085 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1263        1576 :       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..4b5ba87fa1f1ac4a2c3f41a0b8736549da563101 GIT binary patch literal 3773 zcmV;u4npyXP)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+LqRDVJBLbB+!%>9qAVr1DIbC=;oB7sp zCiBrZ>ph4jNG`=aBj-+UE1^Jj9lyb@?R}dz#$D01mwhsE3+4^m(#x0A*R6K~N;e$S zn<~&_goPW9_jfJ{xYrj@gvix2xEiQ#o^MdIvvWIyT;T13flSE7h}9wFVky$f%w2|$ z3=LmM-Hu=OK^pE?hMH+BIV<%nM=A7Os1-)rj_e{kw?3nFq?d~TzBFn!H#MLS!vSZ( z13Pc8hbd$}z|@`piAil1auzr4ioz7J9%BRFw~PwpG7SXm8GMAifs}a-KKavqqs=i6 z=0>o=9u~oOXh6O=#K5b2zJl};dpE-dh?zC7&Qz`W~`Nz*rrPwkz9j~`utklf=n))mwR`$y3|7W+rh zJqGha@g7#zQ}DoF>M5EXISKP2by9!aQ^1e*NwR%Zm`|_d0@DilTh*-HF@)g6L8a8+ zq-MPG~{hp^5l#CJ$drQ z!6i(G-!l&)x;6GYcU8UlK3roLR)}kkO4RzE*+oc6pqk>Gq?St&rjXlEWNwvjKv4(O zQcSt|e2RZkf4|hND9&k-3n*%UJs)xq^Ll@7FU?wG3Ev}idk63*btOd|P)o6&+e;l| z&7-IRc5r*r=CB2AGGNw*5n%sdwRb<*XJe&Dg*zz{P?auZD9S)pOWySJ=AQe_P|uK* zF6xTYJMDDm9v_n6bCkV%(Y~h!t6x(aiCTyv|WhRFy zR1uV#;;Esn6V)Z9@Oi)hI zq9M-ZQNp`7@Ch+@Hq5^8SIZu!_f;pvX6Pv_Yo}(eE_v4*Eg{Kp6y;g-K75r-20!PF zZ=Ue1T*d)9eeq|OT~3}$%lq4()=+xVJOvF7y3V}>o4#mAOB7rC@b + + + + + + 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:2023-12-06 07:59:45Functions: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..8cb8cae8c1 --- /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:2023-12-06 07:59:45Functions: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..44ec8ee5dd --- /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:2023-12-06 07:59:45Functions: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..56012d13f8 --- /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:2023-12-06 07:59:45Functions: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..9f4bbd325e --- /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:2023-12-06 07:59:45Functions: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..5a2dd6dc78 --- /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:2023-12-06 07:59:45Functions: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..3eafac1b4b --- /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:2023-12-06 07:59:45Functions: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&)1
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()18
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)66
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()72
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
+
+
+ + + +
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..6cb5605c31 --- /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:2023-12-06 07:59:45Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()72
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>)42
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&)123
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
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&)1
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&)39318
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)66
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()18
+
+
+ + + +
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..50d4c4ed92 --- /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:2023-12-06 07:59:45Functions: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          42 : 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          42 :   this->common_handlers_  = common_handlers;
+      81          42 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83          42 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85          42 :   nh_ = nh;
+      86             : 
+      87          42 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99          42 :   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          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109          84 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111          42 :   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          42 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122          42 :   is_initialized_ = true;
+     123             : 
+     124          42 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126          42 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133          66 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135          66 :   std::stringstream ss;
+     136             : 
+     137          66 :   is_active_ = true;
+     138             : 
+     139          66 :   ss << "activated";
+     140          66 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142         132 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149          72 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151          72 :   is_active_ = false;
+     152             : 
+     153          72 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154          72 : }
+     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       39318 : 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       39318 :   if (!is_active_) {
+     174       39126 :     return {};
+     175             :   }
+     176             : 
+     177         576 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179         576 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181         384 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183         192 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184         192 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186         192 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187         192 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188         192 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190         192 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191         192 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192         192 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195         192 :     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         192 :   tracker_cmd.use_position_vertical   = true;
+     203         192 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205         192 :   tracker_cmd.use_velocity_vertical   = true;
+     206         192 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208         192 :   tracker_cmd.use_heading = true;
+     209             : 
+     210         192 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212         192 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219          18 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221          18 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223          18 :   tracker_status.active            = is_active_;
+     224          18 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226          18 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233           2 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235           4 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237           2 :   res.message = "callbacks are always disabled";
+     238           2 :   res.success = true;
+     239             : 
+     240           4 :   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         123 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300         246 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302         123 :   res.success = true;
+     303         123 :   res.message = "constraints updated";
+     304             : 
+     305         246 :   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           1 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MidairActivationTracker::setTrajectoryReference([
+     331             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     332           1 :   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          42 : 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..849a451aab --- /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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
<unnamed>66.4 %1111 / 167361.2 %30 / 49
+
+
+ + + + +
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..d9c367504b --- /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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
<unnamed>66.4 %1111 / 167361.2 %30 / 49
+
+
+ + + + +
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..8254e1417c --- /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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
<unnamed>66.4 %1111 / 167361.2 %30 / 49
+
+
+ + + + +
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..11af99dedc --- /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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
+
+
+ + + + +
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..9489ce8d14 --- /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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
+
+
+ + + + +
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..77ded8dcf3 --- /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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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 +
66.4%66.4%
+ 		66.4 %1111 / 167361.2 %30 / 49
+
+
+ + + + +
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..16e23c9189 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html @@ -0,0 +1,276 @@ + + + + + + + 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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
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::checkCollision(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, 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::gotoTrajectoryStartImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(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::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)0
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>)0
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[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::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)1
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()12
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)29
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)29
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)38
mrs_uav_trackers::mpc_tracker::MpcTracker::timerTrajectoryTracking(ros::TimerEvent const&)40
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)42
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)113
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)116
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)121
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)121
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)151
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)180
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)1224
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()2067
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)6211
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()6444
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)20054
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)21721
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()22051
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)22051
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)22051
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()22051
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)22051
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
+
+
+ + + +
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..af8f306091 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html @@ -0,0 +1,276 @@ + + + + + + + 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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()12
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>)42
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)113
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)116
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()22051
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)21721
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)29
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)0
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)1
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)121
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)151
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)22051
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)6211
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)22051
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()22051
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()6444
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)121
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)1
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::gotoTrajectoryStartImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)180
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::timerTrajectoryTracking(ros::TimerEvent const&)40
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&)1224
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)0
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)42
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>)0
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)20054
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::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)29
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)38
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)22051
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()2067
+
+
+ + + +
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..2f47a043a5 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html @@ -0,0 +1,3966 @@ + + + + + + + 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:1111167366.4 %
Date:2023-12-06 07:59:45Functions:304961.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 <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             : using quat_t = Eigen::Quaterniond;
+      45             : 
+      46             : //}
+      47             : 
+      48             : /* using //{ */
+      49             : 
+      50             : using namespace Eigen;
+      51             : 
+      52             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      53             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      54             : 
+      55             : using radians  = mrs_lib::geometry::radians;
+      56             : using sradians = mrs_lib::geometry::sradians;
+      57             : 
+      58             : //}
+      59             : 
+      60             : namespace mrs_uav_trackers
+      61             : {
+      62             : 
+      63             : namespace mpc_tracker
+      64             : {
+      65             : 
+      66             : /* //{ class MpcTracker */
+      67             : 
+      68             : class MpcTracker : public mrs_uav_managers::Tracker {
+      69             : public:
+      70             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      71             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      72             : 
+      73             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd);
+      74             :   void                          deactivate(void);
+      75             :   bool                          resetStatic(void);
+      76             : 
+      77             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState& uav_state, const mrs_uav_managers::Controller::ControlOutput& last_control_output);
+      78             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd);
+      79             :   const mrs_msgs::TrackerStatus             getStatus();
+      80             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      81             : 
+      82             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd);
+      83             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd);
+      84             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd);
+      85             : 
+      86             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      87             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      88             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      89             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      90             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      91             : 
+      92             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+      93             : 
+      94             : private:
+      95             :   ros::NodeHandle nh_;
+      96             : 
+      97             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      98             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      99             : 
+     100             :   std::atomic<bool> callbacks_enabled_ = true;
+     101             : 
+     102             :   std::string _uav_name_;
+     103             : 
+     104             :   // debugging publishers
+     105             :   mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics> pub_diagnostics_;
+     106             :   mrs_lib::PublisherHandler<std_msgs::String>                pub_status_string_;
+     107             : 
+     108             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_processed_trajectory_poses_;
+     109             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_processed_trajectory_markers_;
+     110             : 
+     111             :   mrs_msgs::UavState uav_state_;
+     112             :   std::mutex         mutex_uav_state_;
+     113             : 
+     114             :   bool is_active_      = false;
+     115             :   bool is_initialized_ = false;
+     116             : 
+     117             :   // | --------------------- MPC base params -------------------- |
+     118             : 
+     119             :   int _mpc_n_states_;          // number of states
+     120             :   int _mpc_m_states_;          // number of inputs
+     121             :   int _mpc_n_states_heading_;  // number of states - heading
+     122             :   int _mpc_n_inputs_heading_;  // number of inputs - heading
+     123             :   int _mpc_horizon_len_;       // lenght of the prediction horizon
+     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             :   // trajectory tracking
+     180             :   std::atomic<bool> trajectory_tracking_in_progress_ = false;
+     181             :   int               trajectory_tracking_sub_idx_     = 0;  // increases with every iteration of the simulated model
+     182             :   int               trajectory_tracking_idx_         = 0;  // while tracking, this is the current index in the des_*_whole trajectory
+     183             :   std::mutex        mutex_trajectory_tracking_states_;
+     184             : 
+     185             :   // params of the loaded trajectory
+     186             :   int    trajectory_size_          = 0;
+     187             :   double trajectory_dt_            = 0.2;
+     188             :   bool   trajectory_track_heading_ = false;
+     189             :   bool   trajectory_tracking_loop_ = false;
+     190             :   bool   trajectory_set_           = false;
+     191             :   int    trajectory_count_         = 0;  // counts how many trajectories we have received
+     192             : 
+     193             :   // mpc output
+     194             :   VectorXd   mpc_u_;
+     195             :   double     mpc_u_heading_;
+     196             :   std::mutex mutex_mpc_u_;
+     197             : 
+     198             :   // current state of the dynamical system
+     199             :   MatrixXd   mpc_x_;          // translation state
+     200             :   MatrixXd   mpc_x_heading_;  // heading state
+     201             :   std::mutex mutex_mpc_x_;
+     202             : 
+     203             :   // odometry reset
+     204             :   std::atomic<bool> odometry_reset_in_progress_ = false;
+     205             :   std::atomic<bool> mpc_result_invalid_         = false;
+     206             : 
+     207             :   // predicting the future
+     208             :   MatrixXd   predicted_trajectory_;
+     209             :   MatrixXd   predicted_heading_trajectory_;
+     210             :   std::mutex mutex_predicted_trajectory_;
+     211             : 
+     212             :   mrs_msgs::MpcPredictionFullState prediction_full_state_;
+     213             :   std::mutex                       mutex_prediction_full_state_;
+     214             : 
+     215             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_predicted_trajectory_debugging_;
+     216             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_mpc_reference_debugging_;
+     217             :   mrs_lib::PublisherHandler<geometry_msgs::PoseStamped> ph_current_trajectory_point_;
+     218             : 
+     219             :   std::atomic<bool> mpc_computed_ = false;
+     220             : 
+     221             :   bool brake_ = false;
+     222             : 
+     223             :   // | ----------------------- MPC solver ----------------------- |
+     224             : 
+     225             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_y_;
+     226             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_x_;
+     227             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_z_;
+     228             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_heading_;
+     229             : 
+     230             :   std::mutex mutex_mpc_calculation_;
+     231             : 
+     232             :   int _max_iters_xy_;
+     233             :   int _max_iters_z_;
+     234             :   int _max_iters_heading_;
+     235             : 
+     236             :   // | ----------- measuring the "MPC realtime factor" ---------- |
+     237             : 
+     238             :   double mpc_rtf_ = 0.0;
+     239             : 
+     240             :   // | ------------------- collision avoidance ------------------ |
+     241             : 
+     242             :   // configurable params
+     243             :   bool collision_avoidance_enabled_           = false;
+     244             :   bool collision_avoidance_enabled_passively_ = true;
+     245             : 
+     246             :   // TODO what is this?
+     247             :   double    coef_scaler = 0;
+     248             :   ros::Time coef_time;
+     249             : 
+     250             :   double minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+     251             : 
+     252             :   // params
+     253             :   double                   _avoidance_trajectory_rate_;
+     254             :   double                   _avoidance_radius_threshold_;
+     255             :   double                   _avoidance_z_correction_;
+     256             :   std::string              _avoidance_diagnostics_topic_name_;
+     257             :   std::vector<std::string> _avoidance_other_uav_names_;
+     258             :   double                   _avoidance_z_threshold_;
+     259             : 
+     260             :   // how old can the other UAV trajectory be (since receive time)
+     261             :   double _collision_trajectory_timeout_;
+     262             : 
+     263             :   // when collision detected, slow down during the manouver
+     264             :   double _avoidance_collision_horizontal_speed_coef_;
+     265             : 
+     266             :   // when collision detected, slow down fully this number of steps before it
+     267             :   int _avoidance_collision_slow_down_fully_;
+     268             : 
+     269             :   // when collision detected, start slowing down this number of steps before it
+     270             :   int _avoidance_collision_slow_down_;
+     271             : 
+     272             :   // when avoiding, start climbing this number of steps before it
+     273             :   int _avoidance_collision_start_climbing_;
+     274             : 
+     275             :   int avoidance_this_uav_number_;
+     276             :   int avoidance_this_uav_priority_;
+     277             : 
+     278             :   double            collision_free_altitude_;
+     279             :   std::atomic<bool> avoiding_collision_               = false;
+     280             :   bool              collision_avoidance_affecting_me_ = false;
+     281             : 
+     282             :   // avoidance trajectory will not be published unless we computed it at least once
+     283             :   std::atomic<bool> future_was_predicted_ = false;
+     284             : 
+     285             :   // subscribing to the other UAV future trajectories
+     286             :   void callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg);
+     287             : 
+     288             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>> other_uav_trajectory_subscribers_;
+     289             :   std::map<std::string, mrs_msgs::FutureTrajectory>                  other_uav_avoidance_trajectories_;
+     290             :   std::mutex                                                         mutex_other_uav_avoidance_trajectories_;
+     291             : 
+     292             :   // subscribing to the other UAV diagnostics'
+     293             :   void callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg);
+     294             : 
+     295             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>> other_uav_diag_subscribers_;
+     296             :   std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>                  other_uav_diagnostics_;
+     297             :   std::mutex                                                              mutex_other_uav_diagnostics_;
+     298             : 
+     299             :   bool checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     300             :   bool checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     301             : 
+     302             :   mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory> ph_avoidance_trajectory_;
+     303             : 
+     304             :   ros::ServiceServer service_server_toggle_avoidance_;
+     305             :   bool               callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     306             : 
+     307             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+     308             : 
+     309             :   // | --------------------- MPC calculation -------------------- |
+     310             : 
+     311             :   ros::Timer        timer_mpc_iteration_;
+     312             :   std::atomic<bool> mpc_synchronous_ = false;
+     313             : 
+     314             :   std::atomic<bool> mpc_timer_running_ = false;
+     315             :   void              timerMPC(const ros::TimerEvent& event);
+     316             : 
+     317             :   // | ------------------- trajectory tracking ------------------ |
+     318             : 
+     319             :   ros::Timer timer_trajectory_tracking_;
+     320             :   void       timerTrajectoryTracking(const ros::TimerEvent& event);
+     321             : 
+     322             :   // | -------------------- velocity tracking ------------------- |
+     323             : 
+     324             :   ros::Timer                  timer_velocity_tracking_;
+     325             :   void                        timerVelocityTracking(const ros::TimerEvent& event);
+     326             :   ros::Time                   velocity_reference_time_;
+     327             :   mrs_msgs::VelocityReference velocity_reference_;
+     328             :   std::mutex                  mutex_velocity_reference_;
+     329             :   std::atomic<bool>           velocity_tracking_active_ = false;
+     330             : 
+     331             :   // | ------------------ avoidance trajectory ------------------ |
+     332             : 
+     333             :   ros::Timer timer_avoidance_trajectory_;
+     334             :   void       timerAvoidanceTrajectory(const ros::TimerEvent& event);
+     335             : 
+     336             :   // | ----------------------- diagnostics ---------------------- |
+     337             : 
+     338             :   ros::Timer timer_diagnostics_;
+     339             :   double     _diagnostics_rate_;
+     340             :   void       timerDiagnostics(const ros::TimerEvent& event);
+     341             : 
+     342             :   // | ------------------------ hovering ------------------------ |
+     343             : 
+     344             :   ros::Timer        timer_hover_;
+     345             :   void              timerHover(const ros::TimerEvent& event);
+     346             :   std::atomic<bool> hovering_in_progress_ = false;
+     347             :   void              toggleHover(bool in);
+     348             : 
+     349             :   // | ------------------- trajectory tracking ------------------ |
+     350             : 
+     351             :   std::tuple<bool, std::string> resumeTrajectoryTrackingImpl(void);
+     352             :   std::tuple<bool, std::string> startTrajectoryTrackingImpl(void);
+     353             :   std::tuple<bool, std::string> stopTrajectoryTrackingImpl(void);
+     354             :   std::tuple<bool, std::string> gotoTrajectoryStartImpl(void);
+     355             : 
+     356             :   // | --------------------- other routines --------------------- |
+     357             : 
+     358             :   void publishDiagnostics();
+     359             : 
+     360             :   void debugPrintState(const double throttle);
+     361             :   void debugPrintMPCResult(const double throttle);
+     362             : 
+     363             :   void setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     364             :   void setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     365             :   void setSinglePointReference(const double x, const double y, const double z, const double heading);
+     366             : 
+     367             :   std::tuple<bool, std::string, bool> loadTrajectory(const mrs_msgs::TrajectoryReference msg);
+     368             : 
+     369             :   MatrixXd                       filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed);
+     370             :   std::tuple<MatrixXd, MatrixXd> filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x, double max_speed_y);
+     371             : 
+     372             :   double checkTrajectoryForCollisions(int& first_collision_index);
+     373             : 
+     374             :   void manageConstraints(void);
+     375             :   void calculateMPC(void);
+     376             :   void iterateModel(const double& dt);
+     377             : 
+     378             :   // | ------------------------ profiler ------------------------ |
+     379             : 
+     380             :   mrs_lib::Profiler profiler;
+     381             :   bool              _profiler_enabled_ = false;
+     382             : 
+     383             :   // | ------------------------- wiggle ------------------------- |
+     384             : 
+     385             :   ros::ServiceServer service_server_wiggle_;
+     386             :   bool               callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     387             : 
+     388             :   double wiggle_phase_ = 0;
+     389             : 
+     390             :   // | --------------- dynamic reconfigure server --------------- |
+     391             : 
+     392             :   void dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, uint32_t level);
+     393             : 
+     394             :   boost::recursive_mutex                      config_mutex_;
+     395             :   typedef mrs_uav_trackers::mpc_trackerConfig Config;
+     396             :   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+     397             :   boost::shared_ptr<ReconfigureServer>        reconfigure_server_;
+     398             :   mrs_uav_trackers::mpc_trackerConfig         drs_params_;
+     399             :   std::mutex                                  mutex_drs_params_;
+     400             : };
+     401             : 
+     402             : //}
+     403             : 
+     404             : // | -------------- tracker's interface routines -------------- |
+     405             : 
+     406             : /* //{ initialize() */
+     407             : 
+     408          42 : bool MpcTracker::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     409             :                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     410             : 
+     411          42 :   nh_ = nh;
+     412             : 
+     413          42 :   common_handlers_  = common_handlers;
+     414          42 :   private_handlers_ = private_handlers;
+     415             : 
+     416          42 :   _uav_name_ = common_handlers->uav_name;
+     417             : 
+     418          42 :   ros::Time::waitForValid();
+     419             : 
+     420             :   // --------------------------------------------------------------
+     421             :   // |                     loading parameters                     |
+     422             :   // --------------------------------------------------------------
+     423             : 
+     424             :   // | ---------- loading params using the parent's nh ---------- |
+     425             : 
+     426          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     427             : 
+     428          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     429             : 
+     430          42 :   if (!param_loader_parent.loadedSuccessfully()) {
+     431           0 :     ROS_ERROR("[MpcTracker]: Could not load all parameters!");
+     432           0 :     return false;
+     433             :   }
+     434             : 
+     435             :   // | --------------- loading plugin's parameters -------------- |
+     436             : 
+     437          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/mpc_tracker.yaml");
+     438          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/mpc_tracker.yaml");
+     439             : 
+     440          84 :   const std::string yaml_prefix = "mrs_uav_trackers/mpc_tracker/";
+     441             : 
+     442          42 :   private_handlers->param_loader->loadParam("network/robot_names", _avoidance_other_uav_names_);
+     443             : 
+     444          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/synchronous_rate_limit", _mpc_synchronous_rate_limit_);
+     445          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/asynchronous_loop_rate", _mpc_asynchronous_rate_);
+     446             : 
+     447          42 :   if (_mpc_asynchronous_rate_ < 15) {
+     448           0 :     ROS_ERROR("[MpcTracker]: the asynchronous_loop_rate must be > 15 Hz");
+     449           0 :     return false;
+     450             :   }
+     451             : 
+     452          42 :   dt1_ = 1.0 / _mpc_asynchronous_rate_;
+     453             : 
+     454          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/enabled", drs_params_.braking_enabled);
+     455          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_braking", drs_params_.q_vel_braking);
+     456          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_no_braking", drs_params_.q_vel_no_braking);
+     457             : 
+     458          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/translation/n_states", _mpc_n_states_);
+     459          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/translation/n_inputs", _mpc_m_states_);
+     460          42 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/translation/A", _mat_A_, _mpc_n_states_, _mpc_n_states_);
+     461          42 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/translation/B", _mat_B_, _mpc_n_states_, _mpc_m_states_);
+     462             : 
+     463          42 :   A_ = _mat_A_;
+     464          42 :   B_ = _mat_B_;
+     465             : 
+     466          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/heading/n_states", _mpc_n_states_heading_);
+     467          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "model/heading/n_inputs", _mpc_n_inputs_heading_);
+     468          42 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/heading/A", _mat_A_heading_, _mpc_n_states_heading_, _mpc_n_states_heading_);
+     469          42 :   private_handlers->param_loader->loadMatrixStatic(yaml_prefix + "model/heading/B", _mat_B_heading_, _mpc_n_states_heading_, _mpc_n_inputs_heading_);
+     470             : 
+     471          42 :   A_heading_ = _mat_A_heading_;
+     472          42 :   B_heading_ = _mat_B_heading_;
+     473             : 
+     474             :   // load the MPC parameters
+     475          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/horizon_len", _mpc_horizon_len_);
+     476             : 
+     477          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/dt2", _dt2_);
+     478             : 
+     479          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_rate_);
+     480          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/position_tracking_threshold", _diag_pos_tracking_thr_);
+     481          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/orientation_tracking_threshold", _diag_heading_tracking_thr_);
+     482             : 
+     483          42 :   bool verbose_xy      = false;
+     484          42 :   bool verbose_z       = false;
+     485          42 :   bool verbose_heading = false;
+     486             : 
+     487          84 :   std::vector<double> xy_Q;
+     488          84 :   std::vector<double> z_Q;
+     489          84 :   std::vector<double> heading_Q;
+     490             : 
+     491          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/verbose", verbose_xy);
+     492          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/max_n_iterations", _max_iters_xy_);
+     493          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/Q", xy_Q);
+     494             : 
+     495          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/verbose", verbose_z);
+     496          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/max_n_iterations", _max_iters_z_);
+     497          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/Q", z_Q);
+     498             : 
+     499          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/verbose", verbose_heading);
+     500          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/max_n_iterations", _max_iters_heading_);
+     501          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/Q", heading_Q);
+     502             : 
+     503          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/enabled", drs_params_.wiggle_enabled);
+     504          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/amplitude", drs_params_.wiggle_amplitude);
+     505          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/frequency", drs_params_.wiggle_frequency);
+     506             : 
+     507          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled", collision_avoidance_enabled_);
+     508          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled_passively", collision_avoidance_enabled_passively_);
+     509          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/predicted_trajectory_publish_rate", _avoidance_trajectory_rate_);
+     510          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/correction", _avoidance_z_correction_);
+     511          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/radius", _avoidance_radius_threshold_);
+     512          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/altitude_threshold", _avoidance_z_threshold_);
+     513          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_horizontal_speed_coef", _avoidance_collision_horizontal_speed_coef_);
+     514          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_fully", _avoidance_collision_slow_down_fully_);
+     515          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_start", _avoidance_collision_slow_down_);
+     516          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_start_climbing", _avoidance_collision_start_climbing_);
+     517          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/trajectory_timeout", _collision_trajectory_timeout_);
+     518             : 
+     519          42 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     520           0 :     ROS_ERROR("[MpcTracker]: could not load all parameters!");
+     521           0 :     return false;
+     522             :   }
+     523             : 
+     524          42 :   ROS_INFO_STREAM("[MpcTracker]: initializing solvers with dt1 = " << dt1_);
+     525             : 
+     526          42 :   mpc_solver_y_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_y", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 1);
+     527          42 :   mpc_solver_x_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_x", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 0);
+     528          42 :   mpc_solver_z_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_z", verbose_z, _max_iters_z_, z_Q, dt1_, _dt2_, 2);
+     529             :   mpc_solver_heading_ =
+     530          42 :       std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_hdg", verbose_heading, _max_iters_heading_, heading_Q, dt1_, _dt2_, 0);
+     531             : 
+     532          42 :   mpc_x_         = MatrixXd::Zero(_mpc_n_states_, 1);
+     533          42 :   mpc_x_heading_ = MatrixXd::Zero(_mpc_n_states_heading_, 1);
+     534             : 
+     535          42 :   mpc_u_ = VectorXd::Zero(_mpc_m_states_);
+     536             : 
+     537          42 :   coef_time = ros::Time(0);
+     538             : 
+     539          42 :   des_x_trajectory_       = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     540          42 :   des_y_trajectory_       = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     541          42 :   des_z_trajectory_       = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     542          42 :   des_z_filtered_offset_  = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     543          42 :   des_heading_trajectory_ = MatrixXd::Zero(_mpc_horizon_len_, 1);
+     544             : 
+     545          42 :   service_server_wiggle_ = nh_.advertiseService("wiggle", &MpcTracker::callbackWiggle, this);
+     546             : 
+     547          42 :   pub_diagnostics_   = mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics>(nh_, "diagnostics", 1);
+     548          42 :   pub_status_string_ = mrs_lib::PublisherHandler<std_msgs::String>(nh_, "string", 1);
+     549             : 
+     550             :   // extract the numerical name
+     551          42 :   sscanf(_uav_name_.c_str(), "uav%d", &avoidance_this_uav_number_);
+     552          42 :   ROS_INFO("[MpcTracker]: Numerical ID of this UAV is %d", avoidance_this_uav_number_);
+     553          42 :   avoidance_this_uav_priority_ = avoidance_this_uav_number_;
+     554             : 
+     555             :   // exclude this drone from the list
+     556          42 :   std::vector<std::string>::iterator it = _avoidance_other_uav_names_.begin();
+     557          84 :   while (it != _avoidance_other_uav_names_.end()) {
+     558             : 
+     559          42 :     std::string temp_str = *it;
+     560             : 
+     561             :     int other_uav_priority;
+     562          42 :     sscanf(temp_str.c_str(), "uav%d", &other_uav_priority);
+     563             : 
+     564          42 :     if (other_uav_priority == avoidance_this_uav_number_) {
+     565             : 
+     566          42 :       _avoidance_other_uav_names_.erase(it);
+     567          42 :       continue;
+     568             :     }
+     569             : 
+     570           0 :     it++;
+     571             :   }
+     572             : 
+     573             :   // initialize velocity tracker
+     574             : 
+     575          42 :   velocity_reference_time_ = ros::Time(0);
+     576             : 
+     577             :   // create publishers for predicted trajectory
+     578             : 
+     579          42 :   ph_avoidance_trajectory_           = mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory>(nh_, "predicted_trajectory", 1);
+     580          42 :   ph_predicted_trajectory_debugging_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "predicted_trajectory_debugging", 1);
+     581          42 :   ph_mpc_reference_debugging_        = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "mpc_reference_debugging", 1, true);
+     582          42 :   ph_current_trajectory_point_       = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "current_trajectory_point", 1, true);
+     583             : 
+     584          42 :   pub_debug_processed_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_processed/poses", 1, true);
+     585          42 :   pub_debug_processed_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_processed/markers", 1, true);
+     586             : 
+     587             :   // preallocate predicted trajectory
+     588          42 :   predicted_trajectory_         = MatrixXd::Zero(_mpc_horizon_len_ * _mpc_n_states_, 1);
+     589          42 :   predicted_heading_trajectory_ = MatrixXd::Zero(_mpc_horizon_len_ * _mpc_n_states_, 1);
+     590             : 
+     591          42 :   collision_free_altitude_ = std::numeric_limits<float>::lowest();
+     592             : 
+     593             :   // collision avoidance toggle service
+     594          42 :   service_server_toggle_avoidance_ = nh_.advertiseService("collision_avoidance", &MpcTracker::callbackToggleCollisionAvoidance, this);
+     595             : 
+     596          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+     597          42 :   shopts.nh                 = nh_;
+     598          42 :   shopts.node_name          = "MpcTracker";
+     599          42 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     600          42 :   shopts.threadsafe         = true;
+     601          42 :   shopts.autostart          = true;
+     602          42 :   shopts.queue_size         = 10;
+     603          42 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     604             : 
+     605             :   // create subscribers on other drones diagnostics
+     606          42 :   if (collision_avoidance_enabled_ || collision_avoidance_enabled_passively_) {
+     607             : 
+     608          42 :     for (int i = 0; i < int(_avoidance_other_uav_names_.size()); i++) {
+     609             : 
+     610           0 :       std::string prediction_topic_name = std::string("/") + _avoidance_other_uav_names_[i] + "/control_manager/mpc_tracker/predicted_trajectory";
+     611           0 :       std::string diag_topic_name       = std::string("/") + _avoidance_other_uav_names_[i] + "/control_manager/mpc_tracker/diagnostics";
+     612             : 
+     613           0 :       ROS_INFO("[MpcTracker]: subscribing to %s", prediction_topic_name.c_str());
+     614             : 
+     615           0 :       other_uav_trajectory_subscribers_.push_back(
+     616           0 :           mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>(shopts, prediction_topic_name, &MpcTracker::callbackOtherMavTrajectory, this));
+     617             : 
+     618           0 :       ROS_INFO("[MpcTracker]: subscribing to %s", diag_topic_name.c_str());
+     619             : 
+     620           0 :       other_uav_diag_subscribers_.push_back(
+     621           0 :           mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>(shopts, diag_topic_name, &MpcTracker::callbackOtherMavDiagnostics, this));
+     622             :     }
+     623             :   }
+     624             : 
+     625          42 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, std::string("/") + _uav_name_ + "/estimation_manager/diagnostics");
+     626             : 
+     627             :   // | --------------- dynamic reconfigure server --------------- |
+     628             : 
+     629          42 :   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, nh_));
+     630          42 :   reconfigure_server_->updateConfig(drs_params_);
+     631          42 :   ReconfigureServer::CallbackType f = boost::bind(&MpcTracker::dynamicReconfigureCallback, this, _1, _2);
+     632          42 :   reconfigure_server_->setCallback(f);
+     633             : 
+     634             :   // | ------------------------ profiler ------------------------ |
+     635             : 
+     636          42 :   profiler = mrs_lib::Profiler(common_handlers->parent_nh, "MpcTracker", _profiler_enabled_);
+     637             : 
+     638             :   // | ------------------------- timers ------------------------- |
+     639             : 
+     640          84 :   timer_avoidance_trajectory_ = nh_.createTimer(ros::Rate(_avoidance_trajectory_rate_), &MpcTracker::timerAvoidanceTrajectory, this, false,
+     641          84 :                                                 collision_avoidance_enabled_ || collision_avoidance_enabled_passively_);
+     642          42 :   timer_diagnostics_          = nh_.createTimer(ros::Rate(_diagnostics_rate_), &MpcTracker::timerDiagnostics, this);
+     643          42 :   timer_mpc_iteration_        = nh_.createTimer(ros::Rate(_mpc_asynchronous_rate_), &MpcTracker::timerMPC, this, false, false);
+     644          42 :   timer_trajectory_tracking_  = nh_.createTimer(ros::Rate(1.0), &MpcTracker::timerTrajectoryTracking, this, false, false);
+     645          42 :   timer_velocity_tracking_    = nh_.createTimer(ros::Rate(30.0), &MpcTracker::timerVelocityTracking, this, false, false);
+     646          42 :   timer_hover_                = nh_.createTimer(ros::Rate(10.0), &MpcTracker::timerHover, this, false, false);
+     647             : 
+     648             :   // | ----------------------- finish init ---------------------- |
+     649             : 
+     650          42 :   is_initialized_ = true;
+     651             : 
+     652          42 :   ROS_INFO("[MpcTracker]: initialized");
+     653             : 
+     654          42 :   return true;
+     655             : }
+     656             : 
+     657             : //}
+     658             : 
+     659             : /* //{ activate() */
+     660             : 
+     661          38 : std::tuple<bool, std::string> MpcTracker::activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     662             : 
+     663          76 :   std::stringstream ss;
+     664             : 
+     665          38 :   if (!got_constraints_) {
+     666             : 
+     667           0 :     ss << "can not activate, missing constraints";
+     668           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     669             : 
+     670           0 :     return std::tuple(false, ss.str());
+     671             :   }
+     672             : 
+     673          76 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     674             : 
+     675             :   double uav_state_heading;
+     676             : 
+     677             :   try {
+     678          38 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     679             :   }
+     680           0 :   catch (...) {
+     681           0 :     ss << "could not calculate the UAV heading";
+     682           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     683           0 :     return std::tuple(false, ss.str());
+     684             :   }
+     685             : 
+     686          76 :   MatrixXd mpc_x         = MatrixXd::Zero(_mpc_n_states_, 1);
+     687          38 :   MatrixXd mpc_x_heading = MatrixXd::Zero(_mpc_n_states_heading_, 1);
+     688             : 
+     689          38 :   if (last_tracker_cmd) {
+     690             : 
+     691             :     // set the initial condition from the last tracker's cmd
+     692             : 
+     693          37 :     if (last_tracker_cmd->use_position_horizontal) {
+     694          37 :       mpc_x(0, 0) = last_tracker_cmd->position.x;
+     695          37 :       mpc_x(4, 0) = last_tracker_cmd->position.y;
+     696             :     } else {
+     697           0 :       mpc_x(0, 0) = uav_state.pose.position.x;
+     698           0 :       mpc_x(4, 0) = uav_state.pose.position.y;
+     699             :     }
+     700             : 
+     701          37 :     if (last_tracker_cmd->use_position_vertical) {
+     702          37 :       mpc_x(8, 0) = last_tracker_cmd->position.z;
+     703             :     } else {
+     704           0 :       mpc_x(8, 0) = uav_state.pose.position.z;
+     705             :     }
+     706             : 
+     707          37 :     if (last_tracker_cmd->use_velocity_horizontal) {
+     708          37 :       mpc_x(1, 0) = last_tracker_cmd->velocity.x;
+     709          37 :       mpc_x(5, 0) = last_tracker_cmd->velocity.y;
+     710             :     } else {
+     711           0 :       mpc_x(1, 0) = uav_state.velocity.linear.x;
+     712           0 :       mpc_x(5, 0) = uav_state.velocity.linear.y;
+     713             :     }
+     714             : 
+     715          37 :     if (last_tracker_cmd->use_velocity_vertical) {
+     716          37 :       mpc_x(9, 0) = last_tracker_cmd->velocity.z;
+     717             :     } else {
+     718           0 :       mpc_x(9, 0) = uav_state.velocity.linear.z;
+     719             :     }
+     720             : 
+     721          37 :     if (last_tracker_cmd->use_acceleration) {
+     722           0 :       mpc_x(2, 0)  = last_tracker_cmd->acceleration.x;
+     723           0 :       mpc_x(6, 0)  = last_tracker_cmd->acceleration.y;
+     724           0 :       mpc_x(10, 0) = last_tracker_cmd->acceleration.z;
+     725             :     } else {
+     726          37 :       mpc_x(2, 0)  = 0;
+     727          37 :       mpc_x(6, 0)  = 0;
+     728          37 :       mpc_x(10, 0) = 0;
+     729             :     }
+     730             : 
+     731             :     // the jerks
+     732          37 :     mpc_x(3, 0)  = 0;
+     733          37 :     mpc_x(7, 0)  = 0;
+     734          37 :     mpc_x(11, 0) = 0;
+     735             : 
+     736          37 :     if (last_tracker_cmd->use_heading) {
+     737          37 :       mpc_x_heading(0, 0) = last_tracker_cmd->heading;
+     738           0 :     } else if (last_tracker_cmd->use_orientation) {
+     739             :       try {
+     740           0 :         mpc_x_heading(0, 0) = mrs_lib::AttitudeConverter(last_tracker_cmd->orientation).getHeading();
+     741             :       }
+     742           0 :       catch (...) {
+     743           0 :         mpc_x_heading(0, 0) = uav_state_heading;
+     744             :       }
+     745             :     } else {
+     746           0 :       mpc_x_heading(0, 0) = uav_state_heading;
+     747             :     }
+     748             : 
+     749          37 :     if (last_tracker_cmd->use_heading_rate) {
+     750           6 :       mpc_x_heading(1, 0) = last_tracker_cmd->heading_rate;
+     751             :     } else {
+     752          31 :       mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     753             :     }
+     754             : 
+     755          37 :     mpc_x_heading(2, 0) = 0;
+     756          37 :     mpc_x_heading(3, 0) = 0;
+     757             : 
+     758          37 :     ROS_INFO("[MpcTracker]: activated with last tracker's command");
+     759             : 
+     760             :   } else {
+     761             : 
+     762             :     // set the initial condition completely from the uav_state
+     763             : 
+     764           1 :     mpc_x(0, 0) = uav_state.pose.position.x;
+     765           1 :     mpc_x(1, 0) = uav_state.velocity.linear.x;
+     766           1 :     mpc_x(2, 0) = 0;
+     767           1 :     mpc_x(3, 0) = 0;
+     768             : 
+     769           1 :     mpc_x(4, 0) = uav_state.pose.position.y;
+     770           1 :     mpc_x(5, 0) = uav_state.velocity.linear.y;
+     771           1 :     mpc_x(6, 0) = 0;
+     772           1 :     mpc_x(7, 0) = 0;
+     773             : 
+     774           1 :     mpc_x(8, 0)  = uav_state.pose.position.z;
+     775           1 :     mpc_x(9, 0)  = uav_state.velocity.linear.z;
+     776           1 :     mpc_x(10, 0) = 0;
+     777           1 :     mpc_x(11, 0) = 0;
+     778             : 
+     779           1 :     mpc_x_heading(0, 0) = uav_state_heading;
+     780           1 :     mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     781           1 :     mpc_x_heading(2, 0) = 0;
+     782           1 :     mpc_x_heading(3, 0) = 0;
+     783             : 
+     784           1 :     ROS_INFO("[MpcTracker]: activated with uav state");
+     785             :   }
+     786             : 
+     787             :   {
+     788          76 :     std::scoped_lock lock(mutex_mpc_x_);
+     789             : 
+     790          38 :     mpc_x_         = mpc_x;
+     791          38 :     mpc_x_heading_ = mpc_x_heading;
+     792             :   }
+     793             : 
+     794          38 :   trajectory_tracking_in_progress_ = false;
+     795             : 
+     796          38 :   timer_trajectory_tracking_.stop();
+     797             : 
+     798          38 :   ss << "activated";
+     799          38 :   ROS_INFO_STREAM("[MpcTracker]: " << ss.str());
+     800             : 
+     801             :   // this is here to initialize the desired_trajectory vector
+     802             :   // if deleted (and I tried) the UAV will briefly fly to the
+     803             :   // origin after activation
+     804          38 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     805             : 
+     806          38 :   toggleHover(true);
+     807             : 
+     808          38 :   model_first_iteration_ = true;
+     809             : 
+     810          38 :   A_ = _mat_A_;
+     811          38 :   B_ = _mat_B_;
+     812             : 
+     813          38 :   A_heading_ = _mat_A_heading_;
+     814          38 :   B_heading_ = _mat_B_heading_;
+     815             : 
+     816          38 :   is_active_ = true;
+     817             : 
+     818          38 :   if (!mpc_synchronous_) {
+     819          32 :     timer_mpc_iteration_.start();
+     820             :   }
+     821             : 
+     822          38 :   return std::tuple(true, ss.str());
+     823             : }
+     824             : 
+     825             : //}
+     826             : 
+     827             : /* //{ deactivate() */
+     828             : 
+     829          12 : void MpcTracker::deactivate(void) {
+     830             : 
+     831          12 :   toggleHover(false);
+     832             : 
+     833          12 :   is_active_                       = false;
+     834          12 :   trajectory_tracking_in_progress_ = false;
+     835          12 :   model_first_iteration_           = true;
+     836             : 
+     837          12 :   timer_trajectory_tracking_.stop();
+     838             : 
+     839             :   {
+     840          12 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+     841             : 
+     842          12 :     trajectory_tracking_idx_     = 0;
+     843          12 :     trajectory_tracking_sub_idx_ = 0;
+     844             :   }
+     845             : 
+     846          12 :   ROS_INFO("[MpcTracker]: deactivated");
+     847             : 
+     848          12 :   timer_mpc_iteration_.stop();
+     849             : 
+     850          12 :   publishDiagnostics();
+     851          12 : }
+     852             : 
+     853             : //}
+     854             : 
+     855             : /* //{ resetStatic() */
+     856             : 
+     857           0 : bool MpcTracker::resetStatic(void) {
+     858             : 
+     859           0 :   if (!is_initialized_) {
+     860           0 :     ROS_ERROR("[MpcTracker]: can not reset, not initialized");
+     861           0 :     return false;
+     862             :   }
+     863             : 
+     864           0 :   if (!is_active_) {
+     865           0 :     ROS_ERROR("[MpcTracker]: can not reset, not active");
+     866           0 :     return false;
+     867             :   }
+     868             : 
+     869           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     870             : 
+     871             :   double uav_state_heading;
+     872             : 
+     873             :   try {
+     874           0 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     875             :   }
+     876           0 :   catch (...) {
+     877           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: could not calculate the UAV heading");
+     878           0 :     return false;
+     879             :   }
+     880             : 
+     881             :   {
+     882           0 :     std::scoped_lock lock(mutex_mpc_x_);
+     883             : 
+     884             :     // set the initial condition from the odometry
+     885             : 
+     886           0 :     ROS_INFO("[MpcTracker]: reseting with uav state with no dynamics");
+     887             : 
+     888           0 :     mpc_x_(0, 0) = uav_state.pose.position.x;
+     889           0 :     mpc_x_(1, 0) = 0;
+     890           0 :     mpc_x_(2, 0) = 0;
+     891           0 :     mpc_x_(3, 0) = 0;
+     892             : 
+     893           0 :     mpc_x_(4, 0) = uav_state.pose.position.y;
+     894           0 :     mpc_x_(5, 0) = 0;
+     895           0 :     mpc_x_(6, 0) = 0;
+     896           0 :     mpc_x_(7, 0) = 0;
+     897             : 
+     898           0 :     mpc_x_(8, 0)  = uav_state.pose.position.z;
+     899           0 :     mpc_x_(9, 0)  = 0;
+     900           0 :     mpc_x_(10, 0) = 0;
+     901           0 :     mpc_x_(11, 0) = 0;
+     902             : 
+     903           0 :     mpc_x_heading_(0, 0) = uav_state_heading;
+     904           0 :     mpc_x_heading_(1, 0) = 0;
+     905           0 :     mpc_x_heading_(2, 0) = 0;
+     906           0 :     mpc_x_heading_(3, 0) = 0;
+     907             : 
+     908           0 :     trajectory_tracking_in_progress_ = false;
+     909             : 
+     910           0 :     timer_trajectory_tracking_.stop();
+     911             : 
+     912           0 :     ROS_INFO("[MpcTracker]: reseted");
+     913             :   }
+     914             : 
+     915             :   // this is here to initialize the desired_trajectory vector
+     916             :   // if deleted (and I tried) the UAV will briefly fly to the
+     917             :   // origin after activation
+     918           0 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     919             : 
+     920           0 :   return true;
+     921             : }
+     922             : 
+     923             : //}
+     924             : 
+     925             : /* //{ update() */
+     926             : 
+     927       39318 : std::optional<mrs_msgs::TrackerCommand> MpcTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     928             :                                                            [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     929             : 
+     930      117954 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("update");
+     931      117954 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("MpcTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     932             : 
+     933       78636 :   auto old_uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     934             : 
+     935             :   // calculate dt
+     936       39318 :   double dt = (uav_state.header.stamp - old_uav_state.header.stamp).toSec();
+     937             : 
+     938             :   // save the uav state
+     939       39318 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     940             : 
+     941       39318 :   if (dt > 0) {
+     942             : 
+     943       39318 :     double rate = 1.0 / dt;
+     944             : 
+     945       39318 :     update_rate_ = 0.9 * update_rate_ + 0.1 * rate;
+     946             : 
+     947       39318 :     if (mpc_synchronous_ && (update_rate_ > _mpc_synchronous_rate_limit_)) {
+     948           0 :       mpc_synchronous_ = false;
+     949           0 :       ROS_INFO("[MpcTracker]: detecting high update date (%.1f Hz > %.1f Hz), switching to asynchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     950           0 :       if (is_active_) {
+     951           0 :         timer_mpc_iteration_.start();
+     952             :       }
+     953       39318 :     } else if (!mpc_synchronous_ && (update_rate_ <= _mpc_synchronous_rate_limit_)) {
+     954           6 :       mpc_synchronous_ = true;
+     955           6 :       ROS_INFO("[MpcTracker]: detecting low update rate (%.1f Hz < %.1f Hz), switching to synchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     956           6 :       timer_mpc_iteration_.stop();
+     957             :     }
+     958             :   }
+     959             : 
+     960             :   // up to this part the update() method is evaluated even when the tracker is not active
+     961       39318 :   if (!is_active_) {
+     962       17560 :     return {};
+     963             :   }
+     964             : 
+     965       43516 :   mrs_msgs::TrackerCommand tracker_cmd;
+     966             : 
+     967       21758 :   if (!mpc_synchronous_ && (!mpc_computed_ || mpc_result_invalid_)) {
+     968             : 
+     969          37 :     ROS_WARN_THROTTLE(0.1, "[MpcTracker]: MPC not ready, returning current odom as the command");
+     970             : 
+     971             :     // set the header
+     972          37 :     tracker_cmd.header.stamp    = uav_state.header.stamp;
+     973          37 :     tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     974             : 
+     975             :     // set positions from odom
+     976          37 :     tracker_cmd.position.x              = uav_state.pose.position.x;
+     977          37 :     tracker_cmd.position.y              = uav_state.pose.position.y;
+     978          37 :     tracker_cmd.position.z              = uav_state.pose.position.z;
+     979          37 :     tracker_cmd.use_position_vertical   = 1;
+     980          37 :     tracker_cmd.use_position_horizontal = 1;
+     981             : 
+     982             :     // set velocities from odom
+     983          37 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     984          37 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     985          37 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     986          37 :     tracker_cmd.use_velocity_vertical   = 1;
+     987          37 :     tracker_cmd.use_velocity_horizontal = 1;
+     988             : 
+     989             :     // set zero accelerations
+     990          37 :     tracker_cmd.acceleration.x   = 0;
+     991          37 :     tracker_cmd.acceleration.y   = 0;
+     992          37 :     tracker_cmd.acceleration.z   = 0;
+     993          37 :     tracker_cmd.use_acceleration = 1;
+     994             : 
+     995             :     try {
+     996          37 :       tracker_cmd.heading     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     997          37 :       tracker_cmd.use_heading = 1;
+     998             :     }
+     999           0 :     catch (...) {
+    1000           0 :       tracker_cmd.use_heading = 0;
+    1001           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading");
+    1002             :     }
+    1003             : 
+    1004             :     // set zero jerk
+    1005          37 :     tracker_cmd.jerk.x = 0;
+    1006          37 :     tracker_cmd.jerk.y = 0;
+    1007          37 :     tracker_cmd.jerk.z = 0;
+    1008             : 
+    1009             :     try {
+    1010          37 :       tracker_cmd.heading_rate     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(uav_state.velocity.angular);
+    1011          37 :       tracker_cmd.use_heading_rate = 1;
+    1012             :     }
+    1013           0 :     catch (...) {
+    1014           0 :       tracker_cmd.use_heading_rate = 0;
+    1015           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading rate");
+    1016             :     }
+    1017             : 
+    1018          37 :     return {tracker_cmd};
+    1019             :   }
+    1020             : 
+    1021       21721 :   ros::TimerEvent event;
+    1022             : 
+    1023       21721 :   if (mpc_synchronous_) {
+    1024        1034 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
+    1025        1034 :     timerMPC(event);
+    1026             :   } else {
+    1027       20687 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
+    1028             :   }
+    1029             : 
+    1030       21721 :   if (dt > 0) {
+    1031       21721 :     iterateModel(dt);
+    1032             :   } else {
+    1033           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: dt !> 0, not iterating the model");
+    1034             :   }
+    1035             : 
+    1036       43442 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1037       43442 :   auto prediction_full_state  = mrs_lib::get_mutexed(mutex_prediction_full_state_, prediction_full_state_);
+    1038             : 
+    1039             :   // check whether all outputs are finite
+    1040       21721 :   bool arefinite = true;
+    1041      282373 :   for (int i = 0; i < 12; i++) {
+    1042      260652 :     if (!std::isfinite(mpc_x(i, 0))) {
+    1043           0 :       arefinite = false;
+    1044             :     }
+    1045             :   }
+    1046             : 
+    1047       21721 :   if (arefinite) {
+    1048             : 
+    1049             :     // set the desired states base on the result of the mpc
+    1050       21721 :     tracker_cmd.position.x     = mpc_x(0, 0);
+    1051       21721 :     tracker_cmd.velocity.x     = mpc_x(1, 0);
+    1052       21721 :     tracker_cmd.acceleration.x = mpc_x(2, 0);
+    1053       21721 :     tracker_cmd.jerk.x         = mpc_x(3, 0);
+    1054             : 
+    1055       21721 :     tracker_cmd.position.y     = mpc_x(4, 0);
+    1056       21721 :     tracker_cmd.velocity.y     = mpc_x(5, 0);
+    1057       21721 :     tracker_cmd.acceleration.y = mpc_x(6, 0);
+    1058       21721 :     tracker_cmd.jerk.y         = mpc_x(7, 0);
+    1059             : 
+    1060       21721 :     tracker_cmd.position.z     = mpc_x(8, 0);
+    1061       21721 :     tracker_cmd.velocity.z     = mpc_x(9, 0);
+    1062       21721 :     tracker_cmd.acceleration.z = mpc_x(10, 0);
+    1063       21721 :     tracker_cmd.jerk.z         = mpc_x(11, 0);
+    1064             : 
+    1065       21721 :     tracker_cmd.full_state_prediction = prediction_full_state;
+    1066             : 
+    1067       21721 :     tracker_cmd.use_position_vertical     = 1;
+    1068       21721 :     tracker_cmd.use_position_horizontal   = 1;
+    1069       21721 :     tracker_cmd.use_velocity_vertical     = 1;
+    1070       21721 :     tracker_cmd.use_velocity_horizontal   = 1;
+    1071       21721 :     tracker_cmd.use_acceleration          = 1;
+    1072       21721 :     tracker_cmd.use_jerk                  = 1;
+    1073       21721 :     tracker_cmd.use_full_state_prediction = 1;
+    1074             : 
+    1075             :   } else {
+    1076             : 
+    1077           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC translation outputs are not finite!");
+    1078             : 
+    1079           0 :     return {};
+    1080             :   }
+    1081             : 
+    1082       21721 :   bool heading_finite = true;
+    1083      108605 :   for (int i = 0; i < _mpc_n_states_heading_; i++) {
+    1084       86884 :     if (!std::isfinite(mpc_x_heading(i, 0))) {
+    1085           0 :       heading_finite = false;
+    1086             :     }
+    1087             :   }
+    1088             : 
+    1089       21721 :   if (heading_finite) {
+    1090             : 
+    1091       21721 :     tracker_cmd.heading              = mpc_x_heading(0, 0);
+    1092       21721 :     tracker_cmd.heading_rate         = mpc_x_heading(1, 0);
+    1093       21721 :     tracker_cmd.heading_acceleration = mpc_x_heading(2, 0);
+    1094       21721 :     tracker_cmd.heading_jerk         = mpc_x_heading(3, 0);
+    1095             : 
+    1096       21721 :     tracker_cmd.use_heading              = 1;
+    1097       21721 :     tracker_cmd.use_heading_rate         = 1;
+    1098       21721 :     tracker_cmd.use_heading_acceleration = 1;
+    1099       21721 :     tracker_cmd.use_heading_jerk         = 1;
+    1100             : 
+    1101             :   } else {
+    1102             : 
+    1103           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC heading output is not finite!");
+    1104             : 
+    1105           0 :     return {};
+    1106             :   }
+    1107             : 
+    1108             :   // set the header
+    1109       21721 :   tracker_cmd.header.stamp    = uav_state.header.stamp;
+    1110       21721 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+    1111             : 
+    1112             :   // u have to return a position command
+    1113             :   // can set the jerk to 0
+    1114       21721 :   return {tracker_cmd};
+    1115             : }  // namespace mpc_tracker
+    1116             : 
+    1117             : //}
+    1118             : 
+    1119             : /* //{ getStatus() */
+    1120             : 
+    1121        2067 : const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
+    1122             : 
+    1123        4134 :   auto [mpc_x, mpc_x_heading]  = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1124        2067 :   auto trajectory_size         = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    1125        2067 :   auto trajectory_tracking_idx = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_tracking_idx_);
+    1126             : 
+    1127             :   double des_x, des_y, des_z, des_heading;
+    1128             :   {
+    1129        2067 :     std::scoped_lock lock(mutex_des_trajectory_);
+    1130             : 
+    1131        2067 :     des_x       = des_x_trajectory_(0);
+    1132        2067 :     des_y       = des_y_trajectory_(0);
+    1133        2067 :     des_z       = des_z_trajectory_(0);
+    1134        2067 :     des_heading = des_heading_trajectory_(0);
+    1135             :   }
+    1136             : 
+    1137        2067 :   mrs_msgs::TrackerStatus tracker_status;
+    1138             : 
+    1139        2067 :   tracker_status.active            = is_active_;
+    1140        2067 :   tracker_status.callbacks_enabled = is_active_ && callbacks_enabled_ && !hovering_in_progress_;
+    1141             : 
+    1142        2067 :   tracker_status.tracking_trajectory = trajectory_tracking_in_progress_;
+    1143             : 
+    1144        2067 :   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_;
+    1145        2067 :   bool have_heading_error    = fabs(radians::diff(mpc_x_heading(0), des_heading)) > _diag_heading_tracking_thr_;
+    1146        2067 :   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;
+    1147             : 
+    1148        2067 :   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
+    1149             : 
+    1150        2067 :   tracker_status.trajectory_length = trajectory_size;
+    1151        2067 :   tracker_status.trajectory_idx    = trajectory_tracking_idx;
+    1152             : 
+    1153        2067 :   if (trajectory_tracking_in_progress_) {
+    1154             : 
+    1155         160 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1156             : 
+    1157         160 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    1158             : 
+    1159          80 :     tracker_status.trajectory_reference.header.stamp    = ros::Time::now();
+    1160          80 :     tracker_status.trajectory_reference.header.frame_id = uav_state.header.frame_id;
+    1161             : 
+    1162          80 :     tracker_status.trajectory_reference.reference.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1163          80 :     tracker_status.trajectory_reference.reference.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1164          80 :     tracker_status.trajectory_reference.reference.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1165          80 :     tracker_status.trajectory_reference.reference.heading    = (*des_heading_whole_trajectory_)(trajectory_tracking_idx);
+    1166             : 
+    1167             :     // | ---------- publish the current trajectory point ---------- |
+    1168             : 
+    1169         160 :     geometry_msgs::PoseStamped debug_trajectory_point;
+    1170          80 :     debug_trajectory_point.header.stamp    = ros::Time::now();
+    1171          80 :     debug_trajectory_point.header.frame_id = uav_state_.header.frame_id;
+    1172             : 
+    1173          80 :     debug_trajectory_point.pose.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1174          80 :     debug_trajectory_point.pose.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1175          80 :     debug_trajectory_point.pose.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1176             : 
+    1177          80 :     debug_trajectory_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(trajectory_tracking_idx));
+    1178             : 
+    1179          80 :     ph_current_trajectory_point_.publish(debug_trajectory_point);
+    1180             :   }
+    1181             : 
+    1182        4134 :   return tracker_status;
+    1183             : }
+    1184             : 
+    1185             : //}
+    1186             : 
+    1187             : /* //{ enableCallbacks() */
+    1188             : 
+    1189           2 : const std_srvs::SetBoolResponse::ConstPtr MpcTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+    1190             : 
+    1191           4 :   std::stringstream ss;
+    1192             : 
+    1193           2 :   if (cmd->data != callbacks_enabled_) {
+    1194             : 
+    1195           0 :     callbacks_enabled_ = cmd->data;
+    1196           0 :     ss << "callbacks %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1197             : 
+    1198             :   } else {
+    1199             : 
+    1200           2 :     ss << "callbacks were already %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1201             :   }
+    1202             : 
+    1203           4 :   std_srvs::SetBoolResponse res;
+    1204           2 :   res.message = ss.str();
+    1205           2 :   res.success = true;
+    1206             : 
+    1207           4 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+    1208             : }
+    1209             : 
+    1210             : //}
+    1211             : 
+    1212             : /* switchOdometrySource() //{ */
+    1213             : 
+    1214           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+    1215             : 
+    1216           0 :   odometry_reset_in_progress_ = true;
+    1217           0 :   mpc_result_invalid_         = true;
+    1218             : 
+    1219           0 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1220           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1221             : 
+    1222           0 :   ROS_INFO(
+    1223             :       "[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: "
+    1224             :       "%.2f], "
+    1225             :       "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]",
+    1226             :       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,
+    1227             :       new_uav_state.pose.position.z, new_uav_state.velocity.linear.x, new_uav_state.velocity.linear.y, new_uav_state.velocity.linear.z,
+    1228             :       new_uav_state.acceleration.linear.x, new_uav_state.acceleration.linear.y, new_uav_state.acceleration.linear.z);
+    1229             : 
+    1230           0 :   timer_mpc_iteration_.stop();
+    1231           0 :   ROS_INFO("[MpcTracker]: mpc timer stopped");
+    1232             : 
+    1233           0 :   while (mpc_timer_running_) {
+    1234             : 
+    1235           0 :     ROS_DEBUG("[MpcTracker]: the mpc is in the middle of an iteration, waiting for it to finish");
+    1236           0 :     ros::Duration wait(0.001);
+    1237           0 :     wait.sleep();
+    1238             : 
+    1239           0 :     if (!mpc_timer_running_) {
+    1240           0 :       ROS_DEBUG("[ControlManager]: mpc timer finished");
+    1241           0 :       break;
+    1242             :     }
+    1243             :   }
+    1244             : 
+    1245             :   // | --------- recalculate the goal to new coordinates -------- |
+    1246             : 
+    1247           0 :   double old_heading  = 0;
+    1248           0 :   double new_heading  = 0;
+    1249           0 :   bool   got_headings = true;
+    1250             : 
+    1251             :   try {
+    1252           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1253             :   }
+    1254           0 :   catch (...) {
+    1255           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+    1256           0 :     got_headings = false;
+    1257             :   }
+    1258             : 
+    1259             :   try {
+    1260           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+    1261             :   }
+    1262           0 :   catch (...) {
+    1263           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+    1264           0 :     got_headings = false;
+    1265             :   }
+    1266             : 
+    1267           0 :   std_srvs::TriggerResponse res;
+    1268             : 
+    1269           0 :   if (!got_headings) {
+    1270           0 :     res.message = "could not calculate the heading difference";
+    1271           0 :     res.success = false;
+    1272             : 
+    1273           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1274             :   }
+    1275             : 
+    1276             :   // calculate the difference of position
+    1277           0 :   double dx       = new_uav_state.pose.position.x - uav_state_.pose.position.x;
+    1278           0 :   double dy       = new_uav_state.pose.position.y - uav_state_.pose.position.y;
+    1279           0 :   double dz       = new_uav_state.pose.position.z - uav_state_.pose.position.z;
+    1280           0 :   double dheading = new_heading - old_heading;
+    1281             : 
+    1282           0 :   ROS_INFO("[MpcTracker]: dx %f dy %f dz %f dheading %f", dx, dy, dz, dheading);
+    1283             : 
+    1284             :   {
+    1285           0 :     std::scoped_lock lock(mutex_mpc_x_, mutex_des_trajectory_, mutex_des_whole_trajectory_, mutex_uav_state_);
+    1286             : 
+    1287           0 :     if (trajectory_set_) {
+    1288             : 
+    1289           0 :       for (int i = 0; i < trajectory_size_ + _mpc_horizon_len_; i++) {
+    1290             : 
+    1291           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);
+    1292           0 :         temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1293             : 
+    1294           0 :         (*des_x_whole_trajectory_)(i) = new_uav_state.pose.position.x + temp_vec[0];
+    1295           0 :         (*des_y_whole_trajectory_)(i) = new_uav_state.pose.position.y + temp_vec[1];
+    1296           0 :         (*des_z_whole_trajectory_)(i) += dz;
+    1297           0 :         (*des_heading_whole_trajectory_)(i) += dheading;
+    1298             :       }
+    1299             :     }
+    1300             : 
+    1301           0 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1302             : 
+    1303           0 :       Eigen::Vector2d temp_vec(des_x_trajectory_(i) - uav_state_.pose.position.x, des_y_trajectory_(i) - uav_state_.pose.position.y);
+    1304           0 :       temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1305             : 
+    1306           0 :       des_x_trajectory_(i, 0) = new_uav_state.pose.position.x + temp_vec[0];
+    1307           0 :       des_y_trajectory_(i, 0) = new_uav_state.pose.position.y + temp_vec[1];
+    1308           0 :       des_z_trajectory_(i, 0) += dz;
+    1309           0 :       des_heading_trajectory_(i, 0) += dheading;
+    1310             :     }
+    1311             : 
+    1312             :     // update the position
+    1313             :     {
+    1314           0 :       Eigen::Vector2d temp_vec(mpc_x_(0, 0) - uav_state_.pose.position.x, mpc_x_(4, 0) - uav_state_.pose.position.y);
+    1315           0 :       temp_vec     = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1316           0 :       mpc_x_(0, 0) = new_uav_state.pose.position.x + temp_vec[0];
+    1317           0 :       mpc_x_(4, 0) = new_uav_state.pose.position.y + temp_vec[1];
+    1318           0 :       mpc_x_(8, 0) += dz;
+    1319             :     }
+    1320             : 
+    1321             :     // update the velocity
+    1322             :     {
+    1323           0 :       mpc_x_(1, 0) = new_uav_state.velocity.linear.x;
+    1324           0 :       mpc_x_(5, 0) = new_uav_state.velocity.linear.y;
+    1325             :       // we leave the z velocity as it was in the original frame
+    1326             :     }
+    1327             : 
+    1328             :     // update the acceleration
+    1329             :     {
+    1330           0 :       mpc_x_(2, 0)  = 0;
+    1331           0 :       mpc_x_(6, 0)  = 0;
+    1332           0 :       mpc_x_(10, 0) = 0;
+    1333             :     }
+    1334             : 
+    1335             :     // update the heading and its derivative
+    1336           0 :     mpc_x_heading_(0, 0) += dheading;
+    1337           0 :     mpc_x_heading_(1, 0) = new_uav_state.velocity.angular.x;
+    1338             :   }
+    1339             : 
+    1340           0 :   ROS_INFO(
+    1341             :       "[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: "
+    1342             :       "%.2f]",
+    1343             :       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));
+    1344             : 
+    1345           0 :   ROS_INFO("[MpcTracker]: starting the MPC timer");
+    1346             : 
+    1347           0 :   if (!mpc_synchronous_) {
+    1348           0 :     timer_mpc_iteration_.start();
+    1349             :   }
+    1350             : 
+    1351           0 :   odometry_reset_in_progress_ = false;
+    1352             : 
+    1353           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1354             : }
+    1355             : 
+    1356             : //}
+    1357             : 
+    1358             : /* //{ hover() */
+    1359             : 
+    1360           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1361             : 
+    1362           0 :   toggleHover(true);
+    1363             : 
+    1364           0 :   std::stringstream ss;
+    1365           0 :   ss << "initiating hover";
+    1366             : 
+    1367           0 :   std_srvs::TriggerResponse res;
+    1368           0 :   res.success = true;
+    1369           0 :   res.message = ss.str();
+    1370             : 
+    1371           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1372             : }
+    1373             : 
+    1374             : //}
+    1375             : 
+    1376             : /* //{ startTrajectoryTracking() */
+    1377             : 
+    1378           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1379           0 :   std::stringstream ss;
+    1380             : 
+    1381           0 :   auto [success, message] = startTrajectoryTrackingImpl();
+    1382             : 
+    1383           0 :   std_srvs::TriggerResponse res;
+    1384           0 :   res.success = success;
+    1385           0 :   res.message = message;
+    1386             : 
+    1387           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1388             : }
+    1389             : 
+    1390             : //}
+    1391             : 
+    1392             : /* //{ stopTrajectoryTracking() */
+    1393             : 
+    1394           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1395             : 
+    1396           0 :   auto [success, message] = stopTrajectoryTrackingImpl();
+    1397             : 
+    1398           0 :   std_srvs::TriggerResponse res;
+    1399           0 :   res.success = success;
+    1400           0 :   res.message = message;
+    1401             : 
+    1402           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1403             : }
+    1404             : 
+    1405             : //}
+    1406             : 
+    1407             : /* //{ resumeTrajectoryTracking() */
+    1408             : 
+    1409           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1410             : 
+    1411           0 :   auto [success, message] = resumeTrajectoryTrackingImpl();
+    1412             : 
+    1413           0 :   std_srvs::TriggerResponse res;
+    1414           0 :   res.success = success;
+    1415           0 :   res.message = message;
+    1416             : 
+    1417           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1418             : }
+    1419             : 
+    1420             : //}
+    1421             : 
+    1422             : /* //{ gotoTrajectoryStart() */
+    1423             : 
+    1424           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1425             : 
+    1426           0 :   auto [success, message] = gotoTrajectoryStartImpl();
+    1427             : 
+    1428           0 :   std_srvs::TriggerResponse res;
+    1429           0 :   res.success = success;
+    1430           0 :   res.message = message;
+    1431             : 
+    1432           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1433             : }
+    1434             : 
+    1435             : //}
+    1436             : 
+    1437             : /* //{ setConstraints() */
+    1438             : 
+    1439         123 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+    1440             : 
+    1441         123 :   if (!is_initialized_) {
+    1442           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+    1443             :   }
+    1444             : 
+    1445         123 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+    1446             : 
+    1447             :   // directly updated the speeds in the constraints
+    1448             :   // the reset needs to wait for manageConstraints()
+    1449             :   {
+    1450         246 :     std::scoped_lock lock(mutex_constraints_filtered_);
+    1451             : 
+    1452             :     // important! this needs to be done to initialize the full struct
+    1453         123 :     if (!got_constraints_) {
+    1454             : 
+    1455          42 :       constraints_filtered_ = constraints->constraints;
+    1456             : 
+    1457             :     } else {
+    1458             : 
+    1459          81 :       constraints_filtered_.horizontal_speed          = constraints->constraints.horizontal_speed;
+    1460          81 :       constraints_filtered_.vertical_ascending_speed  = constraints->constraints.vertical_ascending_speed;
+    1461          81 :       constraints_filtered_.vertical_descending_speed = constraints->constraints.vertical_descending_speed;
+    1462          81 :       constraints_filtered_.heading_speed             = constraints->constraints.heading_speed;
+    1463             :     }
+    1464             :   }
+    1465             : 
+    1466         123 :   got_constraints_ = true;
+    1467             : 
+    1468         123 :   all_constraints_set_ = false;
+    1469             : 
+    1470         123 :   ROS_INFO("[MpcTracker]: updating constraints");
+    1471             : 
+    1472         246 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+    1473         123 :   res.success = true;
+    1474         123 :   res.message = "constraints updated";
+    1475             : 
+    1476         123 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+    1477             : }
+    1478             : 
+    1479             : //}
+    1480             : 
+    1481             : /* //{ setReference() */
+    1482             : 
+    1483          29 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MpcTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+    1484             : 
+    1485          29 :   toggleHover(false);
+    1486             : 
+    1487          29 :   setGoal(cmd->reference.position.x, cmd->reference.position.y, cmd->reference.position.z, cmd->reference.heading, true);
+    1488             : 
+    1489          58 :   mrs_msgs::ReferenceSrvResponse res;
+    1490          29 :   res.success = true;
+    1491          29 :   res.message = "reference set";
+    1492             : 
+    1493          58 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+    1494             : }
+    1495             : 
+    1496             : //}
+    1497             : 
+    1498             : /* //{ setVelocityReference() */
+    1499             : 
+    1500           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MpcTracker::setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+    1501             : 
+    1502           0 :   if (!is_initialized_) {
+    1503           0 :     return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+    1504             :   }
+    1505             : 
+    1506             :   {
+    1507           0 :     std::scoped_lock lock(mutex_velocity_reference_);
+    1508             : 
+    1509           0 :     velocity_reference_time_ = ros::Time::now();
+    1510             : 
+    1511           0 :     velocity_reference_ = cmd->reference;
+    1512             :   }
+    1513             : 
+    1514           0 :   if (!velocity_tracking_active_) {
+    1515             : 
+    1516           0 :     ROS_INFO("[MpcTracker]: starting velocity tracking timer");
+    1517             : 
+    1518           0 :     timer_velocity_tracking_.stop();
+    1519           0 :     timer_velocity_tracking_.start();
+    1520             : 
+    1521           0 :     velocity_tracking_active_ = true;
+    1522             :   }
+    1523             : 
+    1524           0 :   mrs_msgs::VelocityReferenceSrvResponse response;
+    1525           0 :   response.success = true;
+    1526           0 :   response.message = "reference set";
+    1527             : 
+    1528           0 :   return mrs_msgs::VelocityReferenceSrvResponse::ConstPtr(new mrs_msgs::VelocityReferenceSrvResponse(response));
+    1529             : }
+    1530             : 
+    1531             : //}
+    1532             : 
+    1533             : /* //{ setTrajectoryReference() */
+    1534             : 
+    1535           1 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MpcTracker::setTrajectoryReference([
+    1536             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+    1537             : 
+    1538           2 :   std::stringstream ss;
+    1539             : 
+    1540           3 :   auto [success, message, modified] = loadTrajectory(cmd->trajectory);
+    1541             : 
+    1542           2 :   mrs_msgs::TrajectoryReferenceSrvResponse response;
+    1543           1 :   response.success  = success;
+    1544           1 :   response.message  = message;
+    1545           1 :   response.modified = modified;
+    1546             : 
+    1547           2 :   return mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr(new mrs_msgs::TrajectoryReferenceSrvResponse(response));
+    1548             : }
+    1549             : 
+    1550             : //}
+    1551             : 
+    1552             : // | ------------------------ callbacks ----------------------- |
+    1553             : 
+    1554             : /* //{ callbackOtherMavTrajectory() */
+    1555             : 
+    1556           0 : void MpcTracker::callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg) {
+    1557             : 
+    1558           0 :   if (!is_initialized_) {
+    1559           0 :     return;
+    1560             :   }
+    1561             : 
+    1562           0 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavTrajectory");
+    1563             :   mrs_lib::ScopeTimer timer =
+    1564           0 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1565             : 
+    1566           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1567             : 
+    1568           0 :   mrs_msgs::FutureTrajectory trajectory = *msg;
+    1569             : 
+    1570             :   // the times might not be synchronized, so just remember the time of receiving it
+    1571           0 :   trajectory.stamp = ros::Time::now();
+    1572             : 
+    1573             :   // transform it from the utm origin to the currently used frame
+    1574           0 :   auto res = common_handlers_->transformer->getTransform("utm_origin", uav_state.header.frame_id, ros::Time::now());
+    1575             : 
+    1576           0 :   if (!res) {
+    1577             : 
+    1578           0 :     std::string message = "[MpcTracker]: can not transform other drone trajectory to the current frame";
+    1579           0 :     ROS_WARN_STREAM_ONCE(message);
+    1580           0 :     ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1581             : 
+    1582           0 :     return;
+    1583             :   }
+    1584             : 
+    1585           0 :   geometry_msgs::TransformStamped tf = res.value();
+    1586             : 
+    1587           0 :   for (int i = 0; i < int(trajectory.points.size()); i++) {
+    1588             : 
+    1589           0 :     geometry_msgs::PoseStamped original_pose;
+    1590             : 
+    1591           0 :     original_pose.pose.position.x = trajectory.points[i].x;
+    1592           0 :     original_pose.pose.position.y = trajectory.points[i].y;
+    1593           0 :     original_pose.pose.position.z = trajectory.points[i].z;
+    1594             : 
+    1595           0 :     original_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1596             : 
+    1597           0 :     auto res = common_handlers_->transformer->transform(original_pose, tf);
+    1598             : 
+    1599           0 :     if (res) {
+    1600           0 :       trajectory.points[i].x = res.value().pose.position.x;
+    1601           0 :       trajectory.points[i].y = res.value().pose.position.y;
+    1602           0 :       trajectory.points[i].z = res.value().pose.position.z;
+    1603             :     } else {
+    1604             : 
+    1605           0 :       std::string message = "[MpcTracker]: could not transform point of other uav future trajectory!";
+    1606           0 :       ROS_WARN_STREAM_ONCE(message);
+    1607           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1608             : 
+    1609           0 :       return;
+    1610             :     }
+    1611             :   }
+    1612             : 
+    1613             :   {
+    1614           0 :     std::scoped_lock lock(mutex_other_uav_avoidance_trajectories_);
+    1615             : 
+    1616             :     // update the diagnostics
+    1617           0 :     other_uav_avoidance_trajectories_[trajectory.uav_name] = trajectory;
+    1618             :   }
+    1619             : }
+    1620             : 
+    1621             : //}
+    1622             : 
+    1623             : /* //{ callbackOtherMavDiagnostics() */
+    1624             : 
+    1625           0 : void MpcTracker::callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg) {
+    1626             : 
+    1627           0 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavDiagnostics");
+    1628             :   mrs_lib::ScopeTimer timer =
+    1629           0 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1630             : 
+    1631           0 :   std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    1632             : 
+    1633           0 :   mrs_msgs::MpcTrackerDiagnostics diagnostics = *msg;
+    1634             : 
+    1635             :   // fill in the current time
+    1636             :   // the other uav's time might not be synchronized with ours
+    1637           0 :   diagnostics.header.stamp = ros::Time::now();
+    1638             : 
+    1639             :   // update the diagnostics
+    1640           0 :   other_uav_diagnostics_[diagnostics.uav_name] = diagnostics;
+    1641           0 : }
+    1642             : 
+    1643             : //}
+    1644             : 
+    1645             : /* //{ callbackToggleCollisionAvoidance() */
+    1646             : 
+    1647           0 : bool MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1648             : 
+    1649           0 :   collision_avoidance_enabled_ = req.data;
+    1650             : 
+    1651           0 :   ROS_INFO("[MpcTracker]: Collision avoidance was switched %s", collision_avoidance_enabled_ ? "TRUE" : "FALSE");
+    1652             : 
+    1653           0 :   res.message = "Collision avoidance set.";
+    1654           0 :   res.success = true;
+    1655             : 
+    1656           0 :   return true;
+    1657             : }
+    1658             : 
+    1659             : //}
+    1660             : 
+    1661             : /* callbackWiggle() //{ */
+    1662             : 
+    1663           0 : bool MpcTracker::callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1664             : 
+    1665           0 :   if (!is_initialized_) {
+    1666             : 
+    1667           0 :     res.success = false;
+    1668           0 :     res.message = "tracker not active";
+    1669           0 :     return true;
+    1670             :   }
+    1671             : 
+    1672             :   {
+    1673           0 :     std::scoped_lock lock(mutex_drs_params_);
+    1674             : 
+    1675           0 :     drs_params_.wiggle_enabled = req.data;
+    1676             : 
+    1677           0 :     reconfigure_server_->updateConfig(drs_params_);
+    1678             :   }
+    1679             : 
+    1680           0 :   res.success = true;
+    1681           0 :   res.message = "wiggle updated";
+    1682             : 
+    1683           0 :   return true;
+    1684             : }
+    1685             : 
+    1686             : //}
+    1687             : 
+    1688             : /* //{ dynamicReconfigureCallback() */
+    1689             : 
+    1690          42 : void MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, [[maybe_unused]] uint32_t level) {
+    1691             : 
+    1692          84 :   std::scoped_lock lock(mutex_drs_params_);
+    1693             : 
+    1694          42 :   drs_params_ = config;
+    1695             : 
+    1696          42 :   ROS_INFO("[MpcTracker]: DRS updated");
+    1697          42 : }
+    1698             : 
+    1699             : //}
+    1700             : 
+    1701             : // --------------------------------------------------------------
+    1702             : // |                          routines                          |
+    1703             : // --------------------------------------------------------------
+    1704             : 
+    1705             : // | --------------- mutual collision avoidance --------------- |
+    1706             : 
+    1707             : /* //{ checkCollision() */
+    1708             : 
+    1709           0 : bool MpcTracker::checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1710             : 
+    1711           0 :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ && fabs(az - bz) < _avoidance_z_threshold_) {
+    1712             : 
+    1713           0 :     return true;
+    1714             : 
+    1715             :   } else {
+    1716             : 
+    1717           0 :     return false;
+    1718             :   }
+    1719             : }
+    1720             : 
+    1721             : //}
+    1722             : 
+    1723             : /* //{ checkCollisionInflated() */
+    1724             : 
+    1725           0 : bool MpcTracker::checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1726             : 
+    1727           0 :   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) {
+    1728             : 
+    1729           0 :     return true;
+    1730             : 
+    1731             :   } else {
+    1732             : 
+    1733           0 :     return false;
+    1734             :   }
+    1735             : }
+    1736             : 
+    1737             : //}
+    1738             : 
+    1739             : /* //{ checkTrajectoryForCollisions() */
+    1740             : 
+    1741             : // Check for potential collisions and return the needed altitude offset to avoid other drones
+    1742       20054 : double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
+    1743             : 
+    1744       20054 :   std::scoped_lock lock(mutex_predicted_trajectory_, mutex_des_trajectory_, mutex_other_uav_avoidance_trajectories_);
+    1745             : 
+    1746       20054 :   first_collision_index = INT_MAX;
+    1747       20054 :   avoiding_collision_   = false;
+    1748             : 
+    1749       20054 :   std::map<std::string, mrs_msgs::FutureTrajectory>::iterator u = other_uav_avoidance_trajectories_.begin();
+    1750             : 
+    1751       20054 :   while (u != other_uav_avoidance_trajectories_.end()) {
+    1752             : 
+    1753             :     // is the other's trajectory fresh enought?
+    1754           0 :     if ((ros::Time::now() - u->second.stamp).toSec() < _collision_trajectory_timeout_) {
+    1755             : 
+    1756           0 :       for (int v = 0; v < _mpc_horizon_len_; v++) {
+    1757             : 
+    1758             :         // check all points of the trajectory for possible collisions
+    1759           0 :         if (checkCollision(predicted_trajectory_(v * _mpc_n_states_, 0), predicted_trajectory_(v * _mpc_n_states_ + 4, 0),
+    1760           0 :                            predicted_trajectory_(v * _mpc_n_states_ + 8, 0), u->second.points[v].x, u->second.points[v].y, u->second.points[v].z)) {
+    1761             : 
+    1762             :           // collision is detected
+    1763           0 :           int other_uav_priority = INT_MAX;
+    1764             :           // get the priority of the other uav
+    1765             :           /* sscanf(u->first.c_str(), "uav%d", &other_uav_priority); */
+    1766           0 :           other_uav_priority = u->second.priority;
+    1767             : 
+    1768             :           // check if we should be avoiding (out priority is higher, or the other uav has collision avoidance turned off)
+    1769           0 :           if ((u->second.collision_avoidance == false) || (other_uav_priority < avoidance_this_uav_priority_)) {
+    1770             : 
+    1771             :             // we should be avoiding
+    1772           0 :             avoiding_collision_      = true;
+    1773           0 :             double tmp_safe_altitude = u->second.points[v].z + _avoidance_z_correction_;
+    1774             : 
+    1775           0 :             if (tmp_safe_altitude > collision_free_altitude_ && v <= _avoidance_collision_start_climbing_) {
+    1776           0 :               collision_free_altitude_ = tmp_safe_altitude;
+    1777             :             }
+    1778             : 
+    1779           0 :             ROS_ERROR_STREAM_THROTTLE(1, "[MpcTracker]: avoiding collision with uav" << other_uav_priority);
+    1780             : 
+    1781             :           } else {
+    1782             :             // the other uav should avoid us
+    1783           0 :             ROS_WARN_STREAM_THROTTLE(1, "[MpcTracker]: detected collision with uav" << other_uav_priority << ", not avoiding (my priority is higher)");
+    1784             :           }
+    1785             :         }
+    1786             : 
+    1787           0 :         if (checkCollisionInflated(predicted_trajectory_(v * _mpc_n_states_, 0), predicted_trajectory_(v * _mpc_n_states_ + 4, 0),
+    1788           0 :                                    predicted_trajectory_(v * _mpc_n_states_ + 8, 0), u->second.points[v].x, u->second.points[v].y, u->second.points[v].z)) {
+    1789             : 
+    1790             :           // collision is detected
+    1791           0 :           if (first_collision_index > v) {
+    1792           0 :             first_collision_index = v;
+    1793             :           }
+    1794             :         }
+    1795             :       }
+    1796             :     }
+    1797           0 :     u++;
+    1798             :   }
+    1799       20054 :   if (!avoiding_collision_) {
+    1800             : 
+    1801       20054 :     auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1802             : 
+    1803             :     // we are not avoiding any collisions, so we slowly reduce the collision avoidance offset to return to normal flight
+    1804       20054 :     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
+    1805             : 
+    1806       20054 :     double safety_area_min_z = common_handlers_->safety_area.getMinZ("");
+    1807             : 
+    1808       20054 :     if (collision_free_altitude_ < safety_area_min_z) {
+    1809             : 
+    1810       20054 :       collision_free_altitude_ = safety_area_min_z;
+    1811             :     }
+    1812             :   }
+    1813             : 
+    1814       40108 :   return collision_free_altitude_;
+    1815             : }
+    1816             : 
+    1817             : //}
+    1818             : 
+    1819             : // | ------------------ trajectory filtering ------------------ |
+    1820             : 
+    1821             : /* //{ filterReferenceXY() */
+    1822             : 
+    1823       22051 : std::tuple<MatrixXd, MatrixXd> MpcTracker::filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x,
+    1824             :                                                              double max_speed_y) {
+    1825             : 
+    1826       22051 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1827             : 
+    1828       44102 :   auto mpc_x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1829       22051 :   auto trajectory_dt = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_dt_);
+    1830             : 
+    1831       44102 :   MatrixXd filtered_x_trajectory = MatrixXd::Zero(_mpc_horizon_len_, 1);
+    1832       44102 :   MatrixXd filtered_y_trajectory = MatrixXd::Zero(_mpc_horizon_len_, 1);
+    1833             : 
+    1834             :   double difference_x;
+    1835             :   double difference_y;
+    1836             :   double max_sample_x;
+    1837             :   double max_sample_y;
+    1838             : 
+    1839      904091 :   for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1840             : 
+    1841      882040 :     if (i == 0) {
+    1842       22051 :       max_sample_x = max_speed_x * dt1;
+    1843       22051 :       max_sample_y = max_speed_y * dt1;
+    1844       22051 :       difference_x = des_x_trajectory(i, 0) - mpc_x(0, 0);
+    1845       22051 :       difference_y = des_y_trajectory(i, 0) - mpc_x(4, 0);
+    1846             :     } else {
+    1847      859989 :       max_sample_x = max_speed_x * _dt2_;
+    1848      859989 :       max_sample_y = max_speed_y * _dt2_;
+    1849      859989 :       difference_x = des_x_trajectory(i, 0) - filtered_x_trajectory(i - 1, 0);
+    1850      859989 :       difference_y = des_y_trajectory(i, 0) - filtered_y_trajectory(i - 1, 0);
+    1851             :     }
+    1852             : 
+    1853      882040 :     if (!trajectory_tracking_in_progress_) {
+    1854             : 
+    1855      852720 :       double direction_angle  = atan2(difference_y, difference_x);
+    1856      852720 :       double max_dir_sample_x = abs(max_sample_x * cos(direction_angle));
+    1857      852720 :       double max_dir_sample_y = abs(max_sample_y * sin(direction_angle));
+    1858             : 
+    1859      852720 :       if (max_sample_x > max_dir_sample_x) {
+    1860      173056 :         max_sample_x = max_dir_sample_x;
+    1861             :       }
+    1862      852720 :       if (max_sample_y > max_dir_sample_y) {
+    1863      852715 :         max_sample_y = max_dir_sample_y;
+    1864             :       }
+    1865             : 
+    1866             :       // saturate the difference
+    1867      852720 :       if (difference_x > max_sample_x)
+    1868       39432 :         difference_x = max_sample_x;
+    1869      813288 :       else if (difference_x < -max_sample_x)
+    1870      157327 :         difference_x = -max_sample_x;
+    1871             : 
+    1872      852720 :       if (difference_y > max_sample_y)
+    1873         537 :         difference_y = max_sample_y;
+    1874      852183 :       else if (difference_y < -max_sample_y)
+    1875      157327 :         difference_y = -max_sample_y;
+    1876             :     }
+    1877             : 
+    1878      882040 :     if (i == 0) {
+    1879       22051 :       filtered_x_trajectory(i, 0) = mpc_x(0, 0) + difference_x;
+    1880       22051 :       filtered_y_trajectory(i, 0) = mpc_x(4, 0) + difference_y;
+    1881             :     } else {
+    1882      859989 :       filtered_x_trajectory(i, 0) = filtered_x_trajectory(i - 1, 0) + difference_x;
+    1883      859989 :       filtered_y_trajectory(i, 0) = filtered_y_trajectory(i - 1, 0) + difference_y;
+    1884             :     }
+    1885             :   }
+    1886             : 
+    1887             :   // | ----------------------- add wiggle ----------------------- |
+    1888             : 
+    1889       22051 :   auto [wiggle_enabled, wiggle_amplitude, wiggle_frequency_] =
+    1890       22051 :       mrs_lib::get_mutexed(mutex_drs_params_, drs_params_.wiggle_enabled, drs_params_.wiggle_amplitude, drs_params_.wiggle_frequency);
+    1891             : 
+    1892       22051 :   if (wiggle_enabled) {
+    1893             : 
+    1894           0 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1895           0 :       filtered_x_trajectory(i, 0) += wiggle_amplitude * cos(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1896           0 :       filtered_y_trajectory(i, 0) += wiggle_amplitude * sin(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1897             :     }
+    1898             : 
+    1899           0 :     wiggle_phase_ += wiggle_frequency_ * dt1 * 2 * M_PI;
+    1900             : 
+    1901           0 :     if (wiggle_phase_ > M_PI) {
+    1902           0 :       wiggle_phase_ -= 2 * M_PI;
+    1903             :     }
+    1904             :   }
+    1905             : 
+    1906       44102 :   return std::make_tuple(filtered_x_trajectory, filtered_y_trajectory);
+    1907             : }
+    1908             : 
+    1909             : //}
+    1910             : 
+    1911             : /* //{ filterReferenceZ() */
+    1912             : 
+    1913       22051 : MatrixXd MpcTracker::filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed) {
+    1914             : 
+    1915       44102 :   auto mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1916             : 
+    1917       22051 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1918             : 
+    1919             :   double difference_z;
+    1920             :   double max_sample_z;
+    1921             : 
+    1922       22051 :   MatrixXd filtered_trajectory = MatrixXd::Zero(_mpc_horizon_len_, 1);
+    1923             : 
+    1924       22051 :   double current_z = mpc_x(8, 0);
+    1925             : 
+    1926      904091 :   for (int i = 0; i < _mpc_horizon_len_; i++) {
+    1927             : 
+    1928      882040 :     if (i == 0) {
+    1929             : 
+    1930       22051 :       difference_z = des_z_trajectory(i, 0) - current_z;
+    1931             : 
+    1932       22051 :       if (difference_z > 0) {
+    1933       11625 :         max_sample_z = max_ascending_speed * dt1;
+    1934             :       } else {
+    1935       10426 :         max_sample_z = max_descending_speed * dt1;
+    1936             :       }
+    1937             : 
+    1938             :     } else {
+    1939             : 
+    1940      859989 :       difference_z = des_z_trajectory(i, 0) - filtered_trajectory(i - 1, 0);
+    1941             : 
+    1942      859989 :       if (difference_z > 0) {
+    1943       53311 :         max_sample_z = max_ascending_speed * _dt2_;
+    1944             :       } else {
+    1945      806678 :         max_sample_z = max_descending_speed * _dt2_;
+    1946             :       }
+    1947             :     }
+    1948             : 
+    1949      882040 :     if (!trajectory_tracking_in_progress_) {
+    1950             : 
+    1951             :       // saturate the difference
+    1952      852720 :       if (difference_z > max_sample_z)
+    1953       38918 :         difference_z = max_sample_z;
+    1954      813802 :       else if (difference_z < -max_sample_z)
+    1955        3816 :         difference_z = -max_sample_z;
+    1956             :     }
+    1957             : 
+    1958      882040 :     if (i == 0) {
+    1959       22051 :       filtered_trajectory(i, 0) = current_z + difference_z;
+    1960             :     } else {
+    1961      859989 :       filtered_trajectory(i, 0) = filtered_trajectory(i - 1, 0) + difference_z;
+    1962             :     }
+    1963             :   }
+    1964             : 
+    1965       44102 :   return filtered_trajectory;
+    1966             : }
+    1967             : 
+    1968             : //}
+    1969             : 
+    1970             : /* //{ manageConstraints() */
+    1971             : 
+    1972       22051 : void MpcTracker::manageConstraints() {
+    1973             : 
+    1974       22051 :   if (!got_constraints_) {
+    1975       21993 :     return;
+    1976             :   }
+    1977             : 
+    1978       22051 :   if (all_constraints_set_) {
+    1979       21993 :     return;
+    1980             :   }
+    1981             : 
+    1982          58 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1983         116 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1984             : 
+    1985         116 :   bool can_change = (fabs(mpc_x(1, 0)) < constraints.horizontal_speed) && (fabs(mpc_x(2, 0)) < constraints.horizontal_acceleration) &&
+    1986          58 :                     (fabs(mpc_x(3, 0)) < constraints.horizontal_jerk) && (fabs(mpc_x(5, 0)) < constraints.horizontal_speed) &&
+    1987          58 :                     (fabs(mpc_x(6, 0)) < constraints.horizontal_acceleration) && (fabs(mpc_x(7, 0)) < constraints.horizontal_jerk) &&
+    1988          58 :                     (mpc_x(9, 0) < constraints.vertical_ascending_speed) && (mpc_x(9, 0) > -constraints.vertical_descending_speed) &&
+    1989          58 :                     (mpc_x(10, 0) < constraints.vertical_ascending_acceleration) && (mpc_x(10, 0) > -constraints.vertical_descending_acceleration) &&
+    1990          58 :                     (mpc_x(11, 0) < constraints.vertical_ascending_jerk) && (mpc_x(11, 0) > -constraints.vertical_descending_jerk) &&
+    1991         174 :                     (fabs(mpc_x_heading(1, 0)) < constraints.heading_speed) && (fabs(mpc_x_heading(2, 0)) < constraints.heading_acceleration) &&
+    1992          58 :                     (fabs(mpc_x_heading(3, 0)) < constraints.heading_jerk);
+    1993             : 
+    1994          58 :   if (can_change) {
+    1995             : 
+    1996             :     {
+    1997          58 :       std::scoped_lock lock(mutex_constraints_filtered_);
+    1998             : 
+    1999          58 :       constraints_filtered_.horizontal_acceleration = constraints.horizontal_acceleration;
+    2000          58 :       constraints_filtered_.horizontal_jerk         = constraints.horizontal_jerk;
+    2001          58 :       constraints_filtered_.horizontal_snap         = constraints.horizontal_snap;
+    2002             : 
+    2003          58 :       constraints_filtered_.vertical_ascending_acceleration = constraints.vertical_ascending_acceleration;
+    2004          58 :       constraints_filtered_.vertical_ascending_jerk         = constraints.vertical_ascending_jerk;
+    2005          58 :       constraints_filtered_.vertical_ascending_snap         = constraints.vertical_ascending_snap;
+    2006             : 
+    2007          58 :       constraints_filtered_.vertical_descending_acceleration = constraints.vertical_descending_acceleration;
+    2008          58 :       constraints_filtered_.vertical_descending_jerk         = constraints.vertical_descending_jerk;
+    2009          58 :       constraints_filtered_.vertical_descending_snap         = constraints.vertical_descending_snap;
+    2010             : 
+    2011          58 :       constraints_filtered_.heading_acceleration = constraints.heading_acceleration;
+    2012          58 :       constraints_filtered_.heading_jerk         = constraints.heading_jerk;
+    2013          58 :       constraints_filtered_.heading_snap         = constraints.heading_snap;
+    2014             :     }
+    2015             : 
+    2016          58 :     ROS_INFO_THROTTLE(1.0, "[MpcTracker]: all constraints succesfully applied");
+    2017          58 :     all_constraints_set_ = true;
+    2018             : 
+    2019             :   } else {
+    2020           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: slowing down to apply new constraints");
+    2021             :   }
+    2022             : }
+    2023             : 
+    2024             : //}
+    2025             : 
+    2026             : /* //{ calculateMPC() */
+    2027             : 
+    2028       22051 : void MpcTracker::calculateMPC() {
+    2029             : 
+    2030       22051 :   std::scoped_lock lock(mutex_mpc_calculation_);
+    2031             : 
+    2032       22051 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2033             : 
+    2034       22051 :   ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: MPC calculation dt = " << dt1);
+    2035             : 
+    2036       22051 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_filtered_, constraints_filtered_);
+    2037       22051 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2038       22051 :   auto uav_state              = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2039       22051 :   auto drs_params             = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    2040             : 
+    2041       22051 :   MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    2042             :   {
+    2043       44102 :     std::scoped_lock lock(mutex_des_trajectory_);
+    2044             : 
+    2045       22051 :     des_x_trajectory       = des_x_trajectory_;
+    2046       22051 :     des_y_trajectory       = des_y_trajectory_;
+    2047       22051 :     des_z_trajectory       = des_z_trajectory_;
+    2048       22051 :     des_heading_trajectory = des_heading_trajectory_;
+    2049             :   }
+    2050             : 
+    2051       22051 :   int    first_collision_index = INT_MAX;
+    2052       22051 :   double lowest_z              = std::numeric_limits<double>::max();
+    2053             : 
+    2054       22051 :   if (collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    2055             : 
+    2056             :     // determine the lowest point in our trajectory
+    2057      822214 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2058      802160 :       if (des_z_trajectory_(i, 0) < lowest_z) {
+    2059       20054 :         lowest_z = des_z_trajectory_(i, 0);
+    2060             :       }
+    2061             :     }
+    2062             : 
+    2063             :     // check other drone trajectories for collisions
+    2064       20054 :     minimum_collison_free_altitude_ = checkTrajectoryForCollisions(first_collision_index);
+    2065             : 
+    2066             :   } else {
+    2067             : 
+    2068        1997 :     minimum_collison_free_altitude_ = common_handlers_->safety_area.getMinZ("");
+    2069             :   }
+    2070             : 
+    2071       22051 :   double max_speed_x = constraints.horizontal_speed;
+    2072       22051 :   double max_speed_y = constraints.horizontal_speed;
+    2073       22051 :   double max_speed_z = constraints.vertical_ascending_speed;
+    2074       22051 :   double min_speed_z = constraints.vertical_descending_speed;
+    2075             : 
+    2076       22051 :   double max_acc_x = constraints.horizontal_acceleration;
+    2077       22051 :   double max_acc_y = constraints.horizontal_acceleration;
+    2078       22051 :   double max_acc_z = constraints.vertical_ascending_acceleration;
+    2079       22051 :   double min_acc_z = constraints.vertical_descending_acceleration;
+    2080             : 
+    2081       22051 :   double max_snap_x = constraints.horizontal_snap;
+    2082       22051 :   double max_snap_y = constraints.horizontal_snap;
+    2083       22051 :   double max_snap_z = constraints.vertical_ascending_snap;
+    2084       22051 :   double min_snap_z = constraints.vertical_descending_snap;
+    2085             : 
+    2086       22051 :   double max_jerk_x = constraints.horizontal_jerk;
+    2087       22051 :   double max_jerk_y = constraints.horizontal_jerk;
+    2088       22051 :   double max_jerk_z = constraints.vertical_ascending_jerk;
+    2089       22051 :   double min_jerk_z = constraints.vertical_descending_jerk;
+    2090             : 
+    2091       22051 :   collision_avoidance_affecting_me_ = false;
+    2092             : 
+    2093       22051 :   if (first_collision_index < _mpc_horizon_len_) {
+    2094             : 
+    2095           0 :     collision_avoidance_affecting_me_ = true;
+    2096             :     // the tmp variable is used to scale the speed of our drone in collision avoidance, depending on how far away the collision is
+    2097           0 :     double tmp = 0;
+    2098             : 
+    2099           0 :     if (first_collision_index <= _avoidance_collision_slow_down_fully_) {
+    2100           0 :       tmp = 1;
+    2101           0 :     } else if (first_collision_index <= _avoidance_collision_slow_down_) {
+    2102           0 :       tmp = 1.0 - ((double)(first_collision_index - _avoidance_collision_slow_down_fully_)) /
+    2103           0 :                       (double)(_avoidance_collision_slow_down_ - _avoidance_collision_slow_down_fully_);
+    2104           0 :       tmp = tmp * tmp;
+    2105             :     }
+    2106             : 
+    2107           0 :     if (!std::isfinite(tmp)) {
+    2108           0 :       tmp = 1.0;
+    2109           0 :       ROS_ERROR("[MpcTracker]: NaN detected in variable 'tmp', setting it to 1.0 and returning!!!");
+    2110           0 :       return;
+    2111           0 :     } else if (tmp > 1.0) {
+    2112           0 :       tmp = 1.0;
+    2113           0 :     } else if (tmp < 0.0) {
+    2114           0 :       tmp = 0.0;
+    2115             :     }
+    2116             : 
+    2117           0 :     if (tmp > coef_scaler) {
+    2118           0 :       coef_scaler = tmp;
+    2119           0 :       coef_time   = ros::Time::now();
+    2120             :     }
+    2121           0 :     if ((ros::Time::now() - coef_time).toSec() > 2.0) {
+    2122           0 :       coef_scaler = tmp;
+    2123             :     }
+    2124             : 
+    2125             :     // we are close to a possible collision, better slow down a bit to give everyone more time
+    2126           0 :     max_speed_x = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2127           0 :     max_speed_y = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2128             :   }
+    2129             : 
+    2130       22051 :   if (collision_free_altitude_ > lowest_z) {
+    2131             : 
+    2132           0 :     collision_avoidance_affecting_me_ = true;
+    2133           0 :     max_speed_x                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2134           0 :     max_speed_y                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2135             :   }
+    2136             : 
+    2137             :   // first control input generated by MPC
+    2138       44102 :   VectorXd mpc_u         = VectorXd::Zero(_mpc_m_states_);
+    2139       22051 :   double   mpc_u_heading = 0;
+    2140             : 
+    2141       22051 :   double iters_z       = 0;
+    2142       22051 :   double iters_x       = 0;
+    2143       22051 :   double iters_y       = 0;
+    2144       22051 :   double iters_heading = 0;
+    2145             : 
+    2146       22051 :   ros::Time time_begin = ros::Time::now();
+    2147             : 
+    2148       44102 :   MatrixXd des_z_filtered = filterReferenceZ(des_z_trajectory, max_speed_z, min_speed_z);
+    2149             : 
+    2150      904091 :   for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2151      882040 :     if (des_z_filtered(i, 0) < minimum_collison_free_altitude_) {
+    2152           0 :       des_z_filtered_offset_(i, 0) = minimum_collison_free_altitude_;
+    2153             :     } else {
+    2154      882040 :       des_z_filtered_offset_(i, 0) = des_z_filtered(i, 0);
+    2155             :     }
+    2156             :   }
+    2157             : 
+    2158             :   // | ----------------- prepare the references ----------------- |
+    2159             : 
+    2160             :   // | -------------------- MPC solver z-axis ------------------- |
+    2161             : 
+    2162       22051 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2163       12327 :     mpc_solver_z_->setVelQ(drs_params.q_vel_braking);
+    2164             :   } else {
+    2165        9724 :     mpc_solver_z_->setVelQ(drs_params.q_vel_no_braking);
+    2166             :   }
+    2167             : 
+    2168       44102 :   MatrixXd initial_z = MatrixXd::Zero(4, 1);
+    2169             : 
+    2170       22051 :   initial_z(0, 0) = mpc_x(8, 0);
+    2171       22051 :   initial_z(1, 0) = mpc_x(9, 0);
+    2172       22051 :   initial_z(2, 0) = mpc_x(10, 0);
+    2173       22051 :   initial_z(3, 0) = mpc_x(11, 0);
+    2174             : 
+    2175       22051 :   mpc_solver_z_->setDt(dt1);
+    2176       22051 :   mpc_solver_z_->setInitialState(initial_z);
+    2177       22051 :   mpc_solver_z_->loadReference(des_z_filtered_offset_);
+    2178       22051 :   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);
+    2179       22051 :   iters_z += mpc_solver_z_->solveMPC();
+    2180             : 
+    2181             :   {
+    2182       44102 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2183             : 
+    2184       22051 :     mpc_solver_z_->getStates(predicted_trajectory_);
+    2185             :   }
+    2186             : 
+    2187       22051 :   mpc_u(2) = mpc_solver_z_->getFirstControlInput();
+    2188             : 
+    2189             :   // if we are climbing to avoid a collision, reduce or arrest our horizontal velocity
+    2190             :   double ascend;
+    2191             :   {
+    2192       22051 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2193             : 
+    2194       22051 :     ascend = (predicted_trajectory_(10, 0) / max_speed_z);
+    2195             :   }
+    2196             : 
+    2197       22051 :   if (ascend > 0 && collision_free_altitude_ > lowest_z) {
+    2198           0 :     max_speed_y = max_speed_y * (1.0 - ascend);
+    2199           0 :     max_speed_x = max_speed_x * (1.0 - ascend);
+    2200             :   }
+    2201             : 
+    2202       66153 :   auto [des_x_filtered, des_y_filtered] = filterReferenceXY(des_x_trajectory, des_y_trajectory, max_speed_x, max_speed_y);
+    2203             : 
+    2204             :   // unwrap the heading reference
+    2205             : 
+    2206       22051 :   des_heading_trajectory(0, 0) = sradians::unwrap(des_heading_trajectory(0, 0), mpc_x_heading(0));
+    2207             : 
+    2208      882040 :   for (int i = 1; i < _mpc_horizon_len_; i++) {
+    2209      859989 :     des_heading_trajectory(i, 0) = sradians::unwrap(des_heading_trajectory(i, 0), des_heading_trajectory(i - 1, 0));
+    2210             :   }
+    2211             : 
+    2212             :   // | -------------------- MPC solver x-axis ------------------- |
+    2213             : 
+    2214       22051 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2215       12327 :     mpc_solver_x_->setVelQ(drs_params.q_vel_braking);
+    2216             :   } else {
+    2217        9724 :     mpc_solver_x_->setVelQ(drs_params.q_vel_no_braking);
+    2218             :   }
+    2219             : 
+    2220       44102 :   MatrixXd initial_x = MatrixXd::Zero(4, 1);
+    2221             : 
+    2222       22051 :   initial_x(0, 0) = mpc_x(0, 0);
+    2223       22051 :   initial_x(1, 0) = mpc_x(1, 0);
+    2224       22051 :   initial_x(2, 0) = mpc_x(2, 0);
+    2225       22051 :   initial_x(3, 0) = mpc_x(3, 0);
+    2226             : 
+    2227       22051 :   mpc_solver_x_->setDt(dt1);
+    2228       22051 :   mpc_solver_x_->setInitialState(initial_x);
+    2229       22051 :   mpc_solver_x_->loadReference(des_x_filtered);
+    2230       22051 :   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);
+    2231       22051 :   iters_x += mpc_solver_x_->solveMPC();
+    2232             : 
+    2233             :   {
+    2234       44102 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2235             : 
+    2236       22051 :     mpc_solver_x_->getStates(predicted_trajectory_);
+    2237             :   }
+    2238             : 
+    2239       22051 :   mpc_u(0) = mpc_solver_x_->getFirstControlInput();
+    2240             : 
+    2241             :   // | -------------------- MPC solver y-axis ------------------- |
+    2242             : 
+    2243       22051 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2244       12328 :     mpc_solver_y_->setVelQ(drs_params.q_vel_braking);
+    2245             :   } else {
+    2246        9723 :     mpc_solver_y_->setVelQ(drs_params.q_vel_no_braking);
+    2247             :   }
+    2248             : 
+    2249       44102 :   MatrixXd initial_y = MatrixXd::Zero(4, 1);
+    2250             : 
+    2251       22051 :   initial_y(0, 0) = mpc_x(4, 0);
+    2252       22051 :   initial_y(1, 0) = mpc_x(5, 0);
+    2253       22051 :   initial_y(2, 0) = mpc_x(6, 0);
+    2254       22051 :   initial_y(3, 0) = mpc_x(7, 0);
+    2255             : 
+    2256       22051 :   mpc_solver_y_->setDt(dt1);
+    2257       22051 :   mpc_solver_y_->setInitialState(initial_y);
+    2258       22051 :   mpc_solver_y_->loadReference(des_y_filtered);
+    2259       22051 :   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);
+    2260       22051 :   iters_y += mpc_solver_y_->solveMPC();
+    2261             :   {
+    2262       44102 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2263             : 
+    2264       22051 :     mpc_solver_y_->getStates(predicted_trajectory_);
+    2265             :   }
+    2266       22051 :   mpc_u(1) = mpc_solver_y_->getFirstControlInput();
+    2267             : 
+    2268             :   // | ------------------- MPC solver heading ------------------- |
+    2269             : 
+    2270       22051 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2271       12328 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_braking);
+    2272             :   } else {
+    2273        9723 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_no_braking);
+    2274             :   }
+    2275             : 
+    2276       22051 :   mpc_solver_heading_->setDt(dt1);
+    2277       22051 :   mpc_solver_heading_->setInitialState(mpc_x_heading);
+    2278       22051 :   mpc_solver_heading_->loadReference(des_heading_trajectory);
+    2279       22051 :   mpc_solver_heading_->setLimits(constraints.heading_speed, constraints.heading_speed, constraints.heading_acceleration, constraints.heading_acceleration,
+    2280             :                                  constraints.heading_jerk, constraints.heading_jerk, constraints.heading_snap, constraints.heading_snap);
+    2281       22051 :   iters_heading += mpc_solver_heading_->solveMPC();
+    2282             :   {
+    2283       44102 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2284             : 
+    2285       22051 :     mpc_solver_heading_->getStates(predicted_heading_trajectory_);
+    2286             :   }
+    2287       22051 :   mpc_u_heading = mpc_solver_heading_->getFirstControlInput();
+    2288             : 
+    2289             :   {
+    2290       22051 :     bool saturating = false;
+    2291             : 
+    2292       22051 :     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       22051 :     if (mpc_u(0) < -max_snap_x * 1.01) {
+    2298           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2299           0 :       mpc_u(0)   = -max_snap_x;
+    2300           0 :       saturating = true;
+    2301             :     }
+    2302       22051 :     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       22051 :     if (mpc_u(1) < -max_snap_y * 1.01) {
+    2308           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2309           0 :       mpc_u(1)   = -max_snap_y;
+    2310           0 :       saturating = true;
+    2311             :     }
+    2312       22051 :     if (mpc_u(2) > max_snap_z * 1.01) {
+    2313           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2314           0 :       mpc_u(2)   = max_snap_z;
+    2315           0 :       saturating = true;
+    2316             :     }
+    2317       22051 :     if (mpc_u(2) < -min_snap_z * 1.01) {
+    2318           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2319           0 :       mpc_u(2)   = -min_snap_z;
+    2320           0 :       saturating = true;
+    2321             :     }
+    2322             : 
+    2323       22051 :     if (saturating) {
+    2324           0 :       debugPrintState(0.1);
+    2325           0 :       debugPrintMPCResult(0.1);
+    2326             :     }
+    2327             :   }
+    2328             : 
+    2329             :   {
+    2330       22051 :     std::scoped_lock lock(mutex_mpc_u_);
+    2331             : 
+    2332       22051 :     mpc_u_         = mpc_u;
+    2333       22051 :     mpc_u_heading_ = mpc_u_heading;
+    2334             :   }
+    2335             : 
+    2336       22051 :   double mpc_solver_time = (ros::Time::now() - time_begin).toSec();
+    2337       22051 :   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_) {
+    2338         528 :     ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: Total MPC solver time: " << mpc_solver_time << " iters X: " << iters_x << "/" << _max_iters_xy_
+    2339             :                                                                            << " iters Y:  " << iters_y << "/" << _max_iters_xy_ << " iters Z: " << iters_z
+    2340             :                                                                            << "/" << _max_iters_z_ << " iters heading: " << iters_heading << "/"
+    2341             :                                                                            << _max_iters_heading_);
+    2342             :   }
+    2343             : 
+    2344       22051 :   future_was_predicted_ = true;
+    2345             : 
+    2346             :   // | ------------- breaking for the next iteration ------------ |
+    2347             : 
+    2348       66153 :   if (drs_params.braking_enabled &&
+    2349       22051 :       (fabs(des_x_filtered(8) - des_x_filtered(_mpc_horizon_len_ - 1)) <= 1e-1 && fabs(des_x_filtered(30) - des_x_filtered(_mpc_horizon_len_ - 1)) <= 1e-1) &&
+    2350       12605 :       (fabs(des_y_filtered(8) - des_y_filtered(_mpc_horizon_len_ - 1)) <= 1e-1 && fabs(des_y_filtered(30) - des_y_filtered(_mpc_horizon_len_ - 1)) <= 1e-1) &&
+    2351       56667 :       (fabs(des_z_filtered(8) - des_z_filtered(_mpc_horizon_len_ - 1)) <= 1e-1 && fabs(des_z_filtered(30) - des_z_filtered(_mpc_horizon_len_ - 1)) <= 1e-1) &&
+    2352       12565 :       (fabs(radians::diff(des_heading_trajectory(10), des_heading_trajectory(_mpc_horizon_len_ - 1))) <= 0.1 &&
+    2353       12565 :        fabs(radians::diff(des_heading_trajectory(30), des_heading_trajectory(_mpc_horizon_len_ - 1))) <= 0.1)) {
+    2354       12565 :     brake_ = true;
+    2355       12565 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: braking");
+    2356             :   } else {
+    2357        9486 :     brake_ = false;
+    2358             :   }
+    2359             : 
+    2360             :   /* publish mpc reference //{ */
+    2361             : 
+    2362             :   {
+    2363       44102 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2364       22051 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2365       22051 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    2366             : 
+    2367             :     {
+    2368       44102 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    2369             : 
+    2370      904091 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2371             : 
+    2372      882040 :         geometry_msgs::Pose new_pose;
+    2373             : 
+    2374      882040 :         new_pose.position.x = des_x_filtered(i, 0);
+    2375      882040 :         new_pose.position.y = des_y_filtered(i, 0);
+    2376      882040 :         new_pose.position.z = des_z_filtered(i, 0);
+    2377             : 
+    2378      882040 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(i));
+    2379             : 
+    2380      882040 :         debug_trajectory_out.poses.push_back(new_pose);
+    2381             :       }
+    2382             :     }
+    2383             : 
+    2384       22051 :     ph_mpc_reference_debugging_.publish(debug_trajectory_out);
+    2385             :   }
+    2386             : 
+    2387             :   //}
+    2388             : }
+    2389             : 
+    2390             : //}
+    2391             : 
+    2392             : /* iterateModel() //{ */
+    2393             : 
+    2394       21721 : void MpcTracker::iterateModel(const double& dt) {
+    2395             : 
+    2396       21721 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2397             : 
+    2398       21721 :   if (model_first_iteration_) {
+    2399             : 
+    2400          38 :     model_iteration_last_time_ = ros::Time::now();
+    2401          38 :     model_first_iteration_     = false;
+    2402             : 
+    2403             :   } else {
+    2404             : 
+    2405       21683 :     dt1 = 0.9 * dt1 + 0.1 * dt;
+    2406             : 
+    2407       21683 :     mrs_lib::set_mutexed(mutex_dt1_, dt1, dt1_);
+    2408       21683 :     timer_mpc_iteration_.setPeriod(ros::Duration(dt1), false);
+    2409             : 
+    2410             :     // clang-format off
+    2411       21683 :     A_ << 1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,           0, 0,   0,           0,
+    2412       21683 :           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,           0, 0,   0,           0,
+    2413       21683 :           0, 0,   1,           dt1,         0, 0,   0,           0,           0, 0,   0,           0,
+    2414       21683 :           0, 0,   0,           1,           0, 0,   0,           0,           0, 0,   0,           0,
+    2415       21683 :           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,
+    2416       21683 :           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,
+    2417       21683 :           0, 0,   0,           0,           0, 0,   1,           dt1,         0, 0,   0,           0,
+    2418       21683 :           0, 0,   0,           0,           0, 0,   0,           1,           0, 0,   0,           0,
+    2419       21683 :           0, 0,   0,           0,           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,
+    2420       21683 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1,
+    2421       21683 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   1,           dt1,
+    2422       21683 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   0,           1;
+    2423             : 
+    2424       21683 :       B_ << 0,   0,   0,
+    2425       21683 :             0,   0,   0,
+    2426       21683 :             0,   0,   0,
+    2427       21683 :             dt1, 0,   0,
+    2428       21683 :             0,   0,   0,
+    2429       21683 :             0,   0,   0,
+    2430       21683 :             0,   0,   0,
+    2431       21683 :             0,   dt1, 0,
+    2432       21683 :             0,   0,   0,
+    2433       21683 :             0,   0,   0,
+    2434       21683 :             0,   0,   0,
+    2435       21683 :             0,   0,   dt1;
+    2436             : 
+    2437       21683 :       A_heading_ << 1, dt1, 0.5*dt1*dt1, 0,
+    2438       21683 :                     0, 1,   dt1,         0.5*dt1*dt1,
+    2439       21683 :                     0, 0,   1,           dt1,
+    2440       21683 :                     0, 0,   0,           1;
+    2441             : 
+    2442       21683 :       B_heading_ << 0,
+    2443       21683 :                     0,
+    2444       43366 :                     0,
+    2445       21683 :                     dt1;
+    2446             : 
+    2447       21683 :     model_iteration_last_time_ = ros::Time::now();
+    2448             :   }
+    2449             : 
+    2450             :   {
+    2451       43442 :     auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2452       43442 :     auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    2453             : 
+    2454       43442 :     MatrixXd new_mpc_x         = A_ * mpc_x + B_ * mpc_u;
+    2455       43442 :     MatrixXd new_mpc_x_heading = A_heading_ * mpc_x_heading + B_heading_ * mpc_u_heading;
+    2456             : 
+    2457             :     // | --------------- check the state difference --------------- |
+    2458             :     {
+    2459       21721 :       auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    2460             : 
+    2461       21721 :       bool problem = false;
+    2462             : 
+    2463             :       // position
+    2464             : 
+    2465       21721 :       if (fabs((new_mpc_x(0) - mpc_x(0)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2466           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(0), new_mpc_x(0),
+    2467             :                   fabs((new_mpc_x(0) - mpc_x(0)) / dt1), constraints.horizontal_speed);
+    2468           0 :         problem = true;
+    2469             :       }
+    2470             : 
+    2471       21721 :       if (fabs((new_mpc_x(4) - mpc_x(4)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2472           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(4), new_mpc_x(4),
+    2473             :                   fabs((new_mpc_x(4) - mpc_x(4)) / dt1), constraints.horizontal_speed);
+    2474           0 :         problem = true;
+    2475             :     }
+    2476             : 
+    2477       21721 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) > 1.05 * constraints.vertical_ascending_speed) {
+    2478           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(8), new_mpc_x(8),
+    2479             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), constraints.vertical_ascending_speed);
+    2480           0 :         problem = true;
+    2481             :       }
+    2482             : 
+    2483       21721 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) < 1.05 * -constraints.vertical_descending_speed) {
+    2484           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(8), new_mpc_x(8),
+    2485             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), -constraints.vertical_descending_speed);
+    2486           0 :         problem = true;
+    2487             :       }
+    2488             : 
+    2489             :       /* if (fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1) > 1.2 * constraints.heading_speed) { */
+    2490             :       /*   ROS_DEBUG("[MpcTracker]: heading update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x_heading(0), new_mpc_x_heading(0), */
+    2491             :       /*             fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1), constraints.heading_speed); */
+    2492             :       /*   problem = true; */
+    2493             :       /* } */
+    2494             : 
+    2495             :       // velocity
+    2496             : 
+    2497       21721 :       if (fabs((new_mpc_x(1) - mpc_x(1)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2498          70 :         ROS_DEBUG("[MpcTracker]: horizontal vel x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(1), new_mpc_x(1),
+    2499             :                   fabs((new_mpc_x(1) - mpc_x(1)) / dt1), constraints.horizontal_acceleration);
+    2500          70 :         problem = true;
+    2501             :       }
+    2502             : 
+    2503       21721 :       if (fabs((new_mpc_x(5) - mpc_x(5)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2504         105 :         ROS_DEBUG("[MpcTracker]: horizontal vel y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(5), new_mpc_x(5),
+    2505             :                   fabs((new_mpc_x(5) - mpc_x(5)) / dt1), constraints.horizontal_acceleration);
+    2506         105 :         problem = true;
+    2507             :       }
+    2508             : 
+    2509       21721 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) > 1.05 * constraints.vertical_ascending_acceleration) {
+    2510         121 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(9), new_mpc_x(9),
+    2511             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), constraints.vertical_ascending_acceleration);
+    2512         121 :         problem = true;
+    2513             :       }
+    2514             : 
+    2515       21721 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) < 1.05 * -constraints.vertical_descending_acceleration) {
+    2516           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(9), new_mpc_x(9),
+    2517             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), -constraints.vertical_descending_acceleration);
+    2518           0 :         problem = true;
+    2519             :       }
+    2520             : 
+    2521             :       // acceleration
+    2522             : 
+    2523       21721 :       if (fabs((new_mpc_x(2) - mpc_x(2)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2524           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(2), new_mpc_x(2),
+    2525             :                   fabs((new_mpc_x(2) - mpc_x(2)) / dt1), constraints.horizontal_jerk);
+    2526           0 :         problem = true;
+    2527             :       }
+    2528             : 
+    2529       21721 :       if (fabs((new_mpc_x(6) - mpc_x(6)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2530           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc y update violates constraints: %.2f -> %.2f, = %.2f > %.2f", mpc_x(6), new_mpc_x(6),
+    2531             :                   fabs((new_mpc_x(6) - mpc_x(6)) / dt1), constraints.horizontal_jerk);
+    2532           0 :         problem = true;
+    2533             :       }
+    2534             : 
+    2535       21721 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) > 1.05 * constraints.vertical_ascending_jerk) {
+    2536           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(10), new_mpc_x(10),
+    2537             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), constraints.vertical_ascending_jerk);
+    2538           0 :         problem = true;
+    2539             :       }
+    2540             : 
+    2541       21721 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) < 1.05 * -constraints.vertical_descending_jerk) {
+    2542           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(10), new_mpc_x(10),
+    2543             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), -constraints.vertical_descending_jerk);
+    2544           0 :         problem = true;
+    2545             :       }
+    2546             : 
+    2547             :       // jerk
+    2548             : 
+    2549       21721 :       if (fabs((new_mpc_x(3) - mpc_x(3)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2550           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(3), new_mpc_x(3),
+    2551             :                   fabs((new_mpc_x(3) - mpc_x(3)) / dt1), constraints.horizontal_snap);
+    2552           0 :         problem = true;
+    2553             :       }
+    2554             : 
+    2555       21721 :       if (fabs((new_mpc_x(7) - mpc_x(7)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2556           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(7), new_mpc_x(7),
+    2557             :                   fabs((new_mpc_x(7) - mpc_x(7)) / dt1), constraints.horizontal_snap);
+    2558           0 :         problem = true;
+    2559             :       }
+    2560             : 
+    2561       21721 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) > 1.05 * constraints.vertical_ascending_snap) {
+    2562           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(11), new_mpc_x(11),
+    2563             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), constraints.vertical_ascending_snap);
+    2564           0 :         problem = true;
+    2565             :       }
+    2566             : 
+    2567       21721 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) < 1.05 * -constraints.vertical_descending_snap) {
+    2568           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(11), new_mpc_x(11),
+    2569             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), -constraints.vertical_descending_snap);
+    2570           0 :         problem = true;
+    2571             :       }
+    2572             : 
+    2573       21721 :       if (problem) {
+    2574         121 :         debugPrintState(0.001);
+    2575         121 :         debugPrintMPCResult(0.001);
+    2576             :       }
+    2577             :     }
+    2578             : 
+    2579             :     {
+    2580       21721 :       std::scoped_lock lock(mutex_mpc_x_);
+    2581             : 
+    2582       21721 :       mpc_x_         = new_mpc_x;
+    2583       21721 :       mpc_x_heading_ = new_mpc_x_heading;
+    2584             : 
+    2585       21721 :       mpc_x_heading_(0) = sradians::wrap(mpc_x_heading_(0));
+    2586             :     }
+    2587             :   }
+    2588       21721 : }
+    2589             : 
+    2590             : //}
+    2591             : 
+    2592             : // | -------------------- referece setting -------------------- |
+    2593             : 
+    2594             : /* //{ loadTrajectory() */
+    2595             : 
+    2596             : // method for setting desired trajectory
+    2597           1 : std::tuple<bool, std::string, bool> MpcTracker::loadTrajectory(const mrs_msgs::TrajectoryReference msg) {
+    2598             : 
+    2599           1 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2600             : 
+    2601             :   // copy the member variables
+    2602           2 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    2603           2 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2604             : 
+    2605           2 :   std::stringstream ss;
+    2606             : 
+    2607             :   /* check the trajectory dt //{ */
+    2608             : 
+    2609             :   double trajectory_dt;
+    2610           1 :   if (msg.dt <= 1e-4) {
+    2611           0 :     trajectory_dt = 0.2;
+    2612           0 :     ROS_WARN_THROTTLE(10.0, "[MpcTracker]: the trajectory dt was not specified, assuming its the old 0.2 s");
+    2613           1 :   } else if (msg.dt < dt1) {
+    2614           0 :     trajectory_dt = 0.2;
+    2615           0 :     ss << std::setprecision(3) << "the trajectory dt (" << msg.dt << " s) is too small (smaller than the tracker's internal step size: " << dt1 << " s)";
+    2616           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2617           0 :     return std::tuple(false, ss.str(), false);
+    2618             :   } else {
+    2619           1 :     trajectory_dt = msg.dt;
+    2620             :   }
+    2621             : 
+    2622             :   //}
+    2623             : 
+    2624           1 :   int trajectory_size = msg.points.size();
+    2625             : 
+    2626             :   /* sanitize the time-ness of the trajectory //{ */
+    2627             : 
+    2628           1 :   int    trajectory_sample_offset    = 0;  // how many samples in past is the trajectory
+    2629           1 :   int    trajectory_subsample_offset = 0;  // how many simulation inner loops ahead of the first valid sample
+    2630           1 :   double trajectory_time_offset      = 0;  // how much time in past in [s]
+    2631             : 
+    2632             :   // btw, "trajectory_time_offset = trajectory_dt*trajectory_sample_offset + _dt1_*trajectory_subsample_offset" should hold
+    2633           1 :   if (msg.fly_now) {
+    2634             : 
+    2635           1 :     ros::Time trajectory_time = msg.header.stamp;
+    2636             : 
+    2637             :     // the desired time is 0 => the current time
+    2638             :     // the trajecoty is a single point => the current time
+    2639           1 :     if (trajectory_time == ros::Time(0) || int(msg.points.size()) == 1) {
+    2640             : 
+    2641           1 :       trajectory_time_offset = 0.0;
+    2642             : 
+    2643             :       // the desired time is specified
+    2644             :     } else {
+    2645             : 
+    2646           0 :       trajectory_time_offset = (ros::Time::now() - trajectory_time).toSec();
+    2647             : 
+    2648             :       // when the time offset is negative, thus in the future
+    2649             :       // just say it, but use it like its from the current time
+    2650           0 :       if (trajectory_time_offset < 0.0) {
+    2651             : 
+    2652           0 :         ROS_WARN_THROTTLE(1.0, "[MpcTracker]: received trajectory with timestamp in the future by %.3f s", -trajectory_time_offset);
+    2653             : 
+    2654           0 :         trajectory_time_offset = 0.0;
+    2655             :       }
+    2656             :     }
+    2657             : 
+    2658             :     // if the time offset is set, check if we need to "move the first idx"
+    2659           1 :     if (trajectory_time_offset > 0.0) {
+    2660             : 
+    2661             :       // calculate the offset in samples
+    2662           0 :       trajectory_sample_offset = int(floor(trajectory_time_offset / trajectory_dt));
+    2663             : 
+    2664             :       // and get the subsample offset, which will be used to initialize the interpolator
+    2665           0 :       trajectory_subsample_offset = int(floor(fmod(trajectory_time_offset, trajectory_dt) / dt1));
+    2666             : 
+    2667           0 :       ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: received trajectory with timestamp in the past by %.3f s",
+    2668             :                          trajectory_dt * trajectory_sample_offset + dt1 * trajectory_subsample_offset);
+    2669             : 
+    2670             :       // if the offset is larger than the number of points in the trajectory
+    2671             :       // the trajectory can not be used
+    2672           0 :       if (trajectory_sample_offset >= trajectory_size) {
+    2673             : 
+    2674           0 :         ss << "trajectory timestamp is too old (time difference = " << trajectory_time_offset << ")";
+    2675           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2676           0 :         return std::tuple(false, ss.str(), false);
+    2677             : 
+    2678             :       } else {
+    2679             : 
+    2680             :         // if the offset is larger than one trajectory sample,
+    2681             :         // offset the start
+    2682           0 :         if (trajectory_time_offset >= trajectory_dt) {
+    2683             : 
+    2684             :           // decrease the trajectory size
+    2685           0 :           trajectory_size -= trajectory_sample_offset;
+    2686             : 
+    2687           0 :           ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: offsetting trajectory by %d samples", trajectory_sample_offset);
+    2688             : 
+    2689             :         } else {
+    2690             : 
+    2691           0 :           trajectory_sample_offset = 0;
+    2692             :         }
+    2693             :       }
+    2694             :     }
+    2695             : 
+    2696             :   } else { // not fly_now
+    2697             : 
+    2698           0 :       trajectory_tracking_in_progress_ = false;
+    2699             : 
+    2700           0 :       timer_trajectory_tracking_.stop();
+    2701             :   }
+    2702             : 
+    2703             :   //}
+    2704             : 
+    2705           1 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory sample offset: %d", trajectory_sample_offset);
+    2706           1 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory subsample offset: %d", trajectory_subsample_offset);
+    2707             : 
+    2708             :   // after this, we should have the correct value of
+    2709             :   // * trajectory_size
+    2710             :   // * trajectory_sample_offset
+    2711             :   // * trajectory_subsample_offset
+    2712             : 
+    2713             :   /* copy the trajectory to a local variable //{ */
+    2714             : 
+    2715             :   // copy only the part from the first valid index
+    2716             : 
+    2717           2 :   MatrixXd des_x_whole_trajectory       = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2718           2 :   MatrixXd des_y_whole_trajectory       = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2719           2 :   MatrixXd des_z_whole_trajectory       = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2720           2 :   MatrixXd des_heading_whole_trajectory = VectorXd::Zero(trajectory_size + _mpc_horizon_len_, 1);
+    2721             : 
+    2722          41 :   for (int i = 0; i < trajectory_size; i++) {
+    2723             : 
+    2724          40 :     des_x_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.x;
+    2725          40 :     des_y_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.y;
+    2726          40 :     des_z_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.z;
+    2727          40 :     des_heading_whole_trajectory(i) = msg.points[trajectory_sample_offset + i].heading;
+    2728             :   }
+    2729             : 
+    2730             :   //}
+    2731             : 
+    2732             :   /* set looping //{ */
+    2733             : 
+    2734           1 :   bool loop = false;
+    2735             : 
+    2736           1 :   if (msg.loop) {
+    2737             : 
+    2738           0 :     double first_x = des_x_whole_trajectory(0);
+    2739           0 :     double first_y = des_y_whole_trajectory(0);
+    2740           0 :     double first_z = des_z_whole_trajectory(0);
+    2741             : 
+    2742           0 :     double last_x = des_x_whole_trajectory(trajectory_size - 1);
+    2743           0 :     double last_y = des_y_whole_trajectory(trajectory_size - 1);
+    2744           0 :     double last_z = des_z_whole_trajectory(trajectory_size - 1);
+    2745             : 
+    2746             :     // check whether the trajectory is loopable
+    2747             :     // TODO should check heading aswell
+    2748           0 :     if (mrs_lib::geometry::dist(vec3_t(first_x, first_y, first_z), vec3_t(last_x, last_y, last_z)) < 3.141592653) {
+    2749             : 
+    2750           0 :       ROS_INFO_THROTTLE(1.0, "[MpcTracker]: looping enabled");
+    2751           0 :       loop = true;
+    2752             : 
+    2753             :     } else {
+    2754             : 
+    2755           0 :       ss << "can not loop trajectory, the first and last points are too far apart";
+    2756           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2757           0 :       return std::tuple(false, ss.str(), false);
+    2758             :     }
+    2759             : 
+    2760             :   } else {
+    2761             : 
+    2762           1 :     loop = false;
+    2763             :   }
+    2764             : 
+    2765             :   //}
+    2766             : 
+    2767             :   // by this time, the values of these should be set:
+    2768             :   // * loop
+    2769             : 
+    2770             :   /* add tail (the last point repeated to fill the prediction horizon) //{ */
+    2771             : 
+    2772           1 :   if (!loop) {
+    2773             : 
+    2774             :     // extend it so it has smooth ending
+    2775          41 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2776             : 
+    2777          40 :       des_x_whole_trajectory(i + trajectory_size)       = des_x_whole_trajectory(i + trajectory_size - 1);
+    2778          40 :       des_y_whole_trajectory(i + trajectory_size)       = des_y_whole_trajectory(i + trajectory_size - 1);
+    2779          40 :       des_z_whole_trajectory(i + trajectory_size)       = des_z_whole_trajectory(i + trajectory_size - 1);
+    2780          40 :       des_heading_whole_trajectory(i + trajectory_size) = des_heading_whole_trajectory(i + trajectory_size - 1);
+    2781             :     }
+    2782             :   }
+    2783             : 
+    2784             :   //}
+    2785             : 
+    2786             :   // by this time, the values of these should be set correctly:
+    2787             :   // * trajectory_size
+    2788             :   // * des_x_whole_trajectory
+    2789             :   // * des_y_whole_trajectory
+    2790             :   // * des_z_whole_trajectory
+    2791             :   // * des_heading_whole_trajectory
+    2792             : 
+    2793             :   /* update the global variables //{ */
+    2794             : 
+    2795             :   {
+    2796           2 :     std::scoped_lock lock(mutex_des_whole_trajectory_, mutex_des_trajectory_, mutex_trajectory_tracking_states_);
+    2797             : 
+    2798           1 :     des_whole_trajectory_id_ = msg.input_id;
+    2799             : 
+    2800           1 :     auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2801             : 
+    2802           1 :     trajectory_tracking_in_progress_ = msg.fly_now;
+    2803           1 :     trajectory_track_heading_        = msg.use_heading;
+    2804             : 
+    2805             :     // allocate the vectors
+    2806           1 :     des_x_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2807           1 :     des_y_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2808           1 :     des_z_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2809           1 :     des_heading_whole_trajectory_ = std::make_shared<VectorXd>(trajectory_size + _mpc_horizon_len_, 1);
+    2810             : 
+    2811          81 :     for (int i = 0; i < trajectory_size + _mpc_horizon_len_; i++) {
+    2812             : 
+    2813          80 :       (*des_x_whole_trajectory_)(i) = des_x_whole_trajectory(i);
+    2814          80 :       (*des_y_whole_trajectory_)(i) = des_y_whole_trajectory(i);
+    2815          80 :       (*des_z_whole_trajectory_)(i) = des_z_whole_trajectory(i);
+    2816             : 
+    2817          80 :       if (trajectory_track_heading_) {
+    2818          80 :         (*des_heading_whole_trajectory_)(i) = des_heading_whole_trajectory(i);
+    2819             :       } else {
+    2820           0 :         (*des_heading_whole_trajectory_).fill(mpc_x_heading(0, 0));
+    2821             :       }
+    2822             :     }
+    2823             : 
+    2824             :     // if we are tracking trajectory, copy the setpoint
+    2825           1 :     if (trajectory_tracking_in_progress_) {
+    2826             : 
+    2827           1 :       toggleHover(false);  // TODO check for deadlock through mutex_des_trajectory_
+    2828             : 
+    2829             :       /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    2830             : 
+    2831          41 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    2832             : 
+    2833          40 :         double first_time = dt1 + i * _dt2_ + trajectory_subsample_offset * dt1;
+    2834             : 
+    2835          40 :         int first_idx  = floor(first_time / trajectory_dt);
+    2836          40 :         int second_idx = first_idx + 1;
+    2837             : 
+    2838          40 :         double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    2839             : 
+    2840          40 :         if (trajectory_tracking_loop_) {
+    2841             : 
+    2842           0 :           if (second_idx >= trajectory_size) {
+    2843           0 :             second_idx -= trajectory_size;
+    2844             :           }
+    2845             : 
+    2846           0 :           if (first_idx >= trajectory_size) {
+    2847           0 :             first_idx -= trajectory_size;
+    2848             :           }
+    2849             :         } else {
+    2850             : 
+    2851          40 :           if (second_idx >= trajectory_size) {
+    2852           1 :             second_idx = trajectory_size - 1;
+    2853             :           }
+    2854             : 
+    2855          40 :           if (first_idx >= trajectory_size) {
+    2856           0 :             first_idx = trajectory_size - 1;
+    2857             :           }
+    2858             :         }
+    2859             : 
+    2860          40 :         des_x_trajectory_(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    2861          40 :         des_y_trajectory_(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    2862          40 :         des_z_trajectory_(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    2863             : 
+    2864          40 :         des_heading_trajectory_(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    2865             :       }
+    2866             : 
+    2867             :       //}
+    2868             :     }
+    2869             : 
+    2870           1 :     trajectory_size_             = trajectory_size;
+    2871           1 :     trajectory_tracking_idx_     = 0;
+    2872           1 :     trajectory_tracking_sub_idx_ = trajectory_subsample_offset;
+    2873           1 :     trajectory_set_              = true;
+    2874           1 :     trajectory_tracking_loop_    = loop;
+    2875           1 :     trajectory_dt_               = trajectory_dt;
+    2876           1 :     trajectory_count_++;
+    2877             : 
+    2878           1 :     timer_trajectory_tracking_.setPeriod(ros::Duration(trajectory_dt));
+    2879             :   }
+    2880             : 
+    2881             :   //}
+    2882             : 
+    2883           1 :   if (trajectory_tracking_in_progress_) {
+    2884           1 :     timer_trajectory_tracking_.start();
+    2885             :   }
+    2886             : 
+    2887           1 :   ROS_INFO_THROTTLE(1, "[MpcTracker]: finished setting trajectory with length %d", trajectory_size);
+    2888             : 
+    2889             :   /* publish the debugging topics of the post-processed trajectory //{ */
+    2890             : 
+    2891             :   {
+    2892             : 
+    2893           2 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2894           1 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2895           1 :     debug_trajectory_out.header.frame_id = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2896             : 
+    2897             :     {
+    2898           2 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2899             : 
+    2900          41 :       for (int i = 0; i < trajectory_size; i++) {
+    2901             : 
+    2902          40 :         geometry_msgs::Pose new_pose;
+    2903             : 
+    2904          40 :         new_pose.position.x = (*des_x_whole_trajectory_)(i);
+    2905          40 :         new_pose.position.y = (*des_y_whole_trajectory_)(i);
+    2906          40 :         new_pose.position.z = (*des_z_whole_trajectory_)(i);
+    2907             : 
+    2908          40 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(i));
+    2909             : 
+    2910          40 :         debug_trajectory_out.poses.push_back(new_pose);
+    2911             :       }
+    2912             :     }
+    2913             : 
+    2914           1 :     pub_debug_processed_trajectory_poses_.publish(debug_trajectory_out);
+    2915             : 
+    2916           2 :     visualization_msgs::MarkerArray msg_out;
+    2917             : 
+    2918           2 :     visualization_msgs::Marker marker;
+    2919             : 
+    2920           1 :     marker.header.stamp     = ros::Time::now();
+    2921           1 :     marker.header.frame_id  = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2922           1 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    2923           1 :     marker.color.a          = 1;
+    2924           1 :     marker.scale.x          = 0.05;
+    2925           1 :     marker.color.r          = 1;
+    2926           1 :     marker.color.g          = 0;
+    2927           1 :     marker.color.b          = 0;
+    2928           1 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2929             : 
+    2930             :     {
+    2931           2 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2932             : 
+    2933          40 :       for (int i = 0; i < trajectory_size - 1; i++) {
+    2934             : 
+    2935          39 :         geometry_msgs::Point point1;
+    2936             : 
+    2937          39 :         point1.x = des_x_whole_trajectory(i);
+    2938          39 :         point1.y = des_y_whole_trajectory(i);
+    2939          39 :         point1.z = des_z_whole_trajectory(i);
+    2940             : 
+    2941          39 :         marker.points.push_back(point1);
+    2942             : 
+    2943          39 :         geometry_msgs::Point point2;
+    2944             : 
+    2945          39 :         point2.x = des_x_whole_trajectory(i + 1);
+    2946          39 :         point2.y = des_y_whole_trajectory(i + 1);
+    2947          39 :         point2.z = des_z_whole_trajectory(i + 1);
+    2948             : 
+    2949          39 :         marker.points.push_back(point2);
+    2950             :       }
+    2951             :     }
+    2952             : 
+    2953           1 :     msg_out.markers.push_back(marker);
+    2954             : 
+    2955           1 :     pub_debug_processed_trajectory_markers_.publish(msg_out);
+    2956             :   }
+    2957             : 
+    2958             :   //}
+    2959             : 
+    2960           1 :   publishDiagnostics();
+    2961             : 
+    2962           2 :   return std::tuple(true, "trajectory loaded", false);
+    2963             : }
+    2964             : 
+    2965             : //}
+    2966             : 
+    2967             : /* //{ setSinglePointReference() */
+    2968             : 
+    2969             : // fill the des_*_trajectory based on a single point
+    2970         180 : void MpcTracker::setSinglePointReference(const double x, const double y, const double z, const double heading) {
+    2971             : 
+    2972         360 :   std::scoped_lock lock(mutex_des_trajectory_);
+    2973             : 
+    2974         180 :   des_x_trajectory_.fill(x);
+    2975         180 :   des_y_trajectory_.fill(y);
+    2976         180 :   des_z_trajectory_.fill(z);
+    2977         180 :   des_heading_trajectory_.fill(heading);
+    2978         180 : }
+    2979             : 
+    2980             : //}
+    2981             : 
+    2982             : /* //{ setGoal() */
+    2983             : 
+    2984             : // set absolute goal
+    2985          29 : void MpcTracker::setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2986             : 
+    2987          29 :   double desired_heading = sradians::wrap(heading);
+    2988             : 
+    2989          58 :   auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2990             : 
+    2991          29 :   if (!use_heading) {
+    2992           0 :     desired_heading = mpc_x_heading(0, 0);
+    2993             :   }
+    2994             : 
+    2995          29 :   trajectory_tracking_in_progress_ = false;
+    2996          29 :   timer_trajectory_tracking_.stop();
+    2997             : 
+    2998          29 :   setSinglePointReference(pos_x, pos_y, pos_z, desired_heading);
+    2999             : 
+    3000          29 :   publishDiagnostics();
+    3001          29 : }
+    3002             : 
+    3003             : //}
+    3004             : 
+    3005             : /* //{ setRelativeGoal() */
+    3006             : 
+    3007         151 : void MpcTracker::setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    3008             : 
+    3009         302 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3010             : 
+    3011         151 :   double abs_x = mpc_x(0, 0) + pos_x;
+    3012         151 :   double abs_y = mpc_x(4, 0) + pos_y;
+    3013         151 :   double abs_z = mpc_x(8, 0) + pos_z;
+    3014             : 
+    3015         151 :   double abs_heading = mpc_x_heading(0, 0);
+    3016             : 
+    3017         151 :   if (use_heading) {
+    3018           0 :     abs_heading += heading;
+    3019             :   }
+    3020             : 
+    3021         151 :   trajectory_tracking_in_progress_ = false;
+    3022         151 :   timer_trajectory_tracking_.stop();
+    3023             : 
+    3024         151 :   setSinglePointReference(abs_x, abs_y, abs_z, abs_heading);
+    3025             : 
+    3026         151 :   publishDiagnostics();
+    3027         151 : }
+    3028             : 
+    3029             : //}
+    3030             : 
+    3031             : /* toggleHover() //{ */
+    3032             : 
+    3033         116 : void MpcTracker::toggleHover(bool in) {
+    3034             : 
+    3035             :   // DON'T PUT MUTEX LOCK IN THIS FUNCTION
+    3036             :   // it is called under mutex elsewhere
+    3037             : 
+    3038         116 :   if (in == false && hovering_in_progress_) {
+    3039             : 
+    3040          38 :     ROS_DEBUG("[MpcTracker]: stoppping the hover timer");
+    3041             : 
+    3042          38 :     timer_hover_.stop();
+    3043             : 
+    3044          38 :     hovering_in_progress_ = false;
+    3045             : 
+    3046          78 :   } else if (in == true && !hovering_in_progress_) {
+    3047             : 
+    3048          38 :     ROS_DEBUG("[MpcTracker]: starting the hover timer");
+    3049             : 
+    3050          38 :     hovering_in_progress_ = true;
+    3051             : 
+    3052          38 :     timer_hover_.start();
+    3053             :   }
+    3054         116 : }
+    3055             : 
+    3056             : //}
+    3057             : 
+    3058             : // | ------------------- trajectory tracking ------------------ |
+    3059             : 
+    3060             : /* startTrajectoryTrackingImpl() //{ */
+    3061             : 
+    3062           0 : std::tuple<bool, std::string> MpcTracker::startTrajectoryTrackingImpl(void) {
+    3063             : 
+    3064           0 :   std::stringstream ss;
+    3065             : 
+    3066           0 :   if (trajectory_set_) {
+    3067             : 
+    3068           0 :     toggleHover(false);
+    3069             : 
+    3070             :     {
+    3071           0 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3072             : 
+    3073           0 :       trajectory_tracking_in_progress_ = true;
+    3074           0 :       trajectory_tracking_idx_         = 0;
+    3075           0 :       trajectory_tracking_sub_idx_     = 0;
+    3076             :     }
+    3077             : 
+    3078           0 :     timer_trajectory_tracking_.setPeriod(ros::Duration(trajectory_dt_));
+    3079           0 :     timer_trajectory_tracking_.start();
+    3080             : 
+    3081           0 :     publishDiagnostics();
+    3082             : 
+    3083           0 :     ss << "trajectory tracking started";
+    3084           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3085             : 
+    3086           0 :     return std::tuple(true, ss.str());
+    3087             : 
+    3088             :   } else {
+    3089             : 
+    3090           0 :     ss << "can not start trajectory tracking, the trajectory is not set";
+    3091           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3092             : 
+    3093           0 :     return std::tuple(false, ss.str());
+    3094             :   }
+    3095             : }
+    3096             : 
+    3097             : //}
+    3098             : 
+    3099             : /* resumeTrajectoryTrackingImpl() //{ */
+    3100             : 
+    3101           0 : std::tuple<bool, std::string> MpcTracker::resumeTrajectoryTrackingImpl(void) {
+    3102             : 
+    3103           0 :   std::stringstream ss;
+    3104             : 
+    3105           0 :   if (trajectory_set_) {
+    3106             : 
+    3107           0 :     toggleHover(false);
+    3108             : 
+    3109           0 :     auto trajectory_tracking_idx = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_tracking_idx_);
+    3110             : 
+    3111           0 :     if (trajectory_tracking_idx < (trajectory_size_ - 1)) {
+    3112             : 
+    3113             :       {
+    3114           0 :         std::scoped_lock lock(mutex_des_trajectory_);
+    3115             : 
+    3116           0 :         trajectory_tracking_in_progress_ = true;
+    3117             :       }
+    3118             : 
+    3119           0 :       timer_trajectory_tracking_.setPeriod(ros::Duration(trajectory_dt_));
+    3120           0 :       timer_trajectory_tracking_.start();
+    3121             : 
+    3122           0 :       ss << "trajectory tracking resumed";
+    3123           0 :       ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3124             : 
+    3125           0 :       publishDiagnostics();
+    3126             : 
+    3127           0 :       return std::tuple(true, ss.str());
+    3128             : 
+    3129             :     } else {
+    3130             : 
+    3131           0 :       ss << "can not resume trajectory tracking, trajectory is already finished";
+    3132           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3133             : 
+    3134           0 :       return std::tuple(false, ss.str());
+    3135             :     }
+    3136             : 
+    3137             :   } else {
+    3138             : 
+    3139           0 :     ss << "can not resume trajectory tracking, ther trajectory is not set";
+    3140           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3141             : 
+    3142           0 :     return std::tuple(false, ss.str());
+    3143             :   }
+    3144             : }
+    3145             : 
+    3146             : //}
+    3147             : 
+    3148             : /* stopTrajectoryTrackingImpl() //{ */
+    3149             : 
+    3150           0 : std::tuple<bool, std::string> MpcTracker::stopTrajectoryTrackingImpl(void) {
+    3151             : 
+    3152           0 :   std::stringstream ss;
+    3153             : 
+    3154           0 :   if (trajectory_tracking_in_progress_) {
+    3155             : 
+    3156           0 :     trajectory_tracking_in_progress_ = false;
+    3157           0 :     timer_trajectory_tracking_.stop();
+    3158             : 
+    3159           0 :     toggleHover(true);
+    3160             : 
+    3161           0 :     ss << "stopping trajectory tracking";
+    3162           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3163             : 
+    3164           0 :     publishDiagnostics();
+    3165             : 
+    3166             :   } else {
+    3167             : 
+    3168           0 :     ss << "can not stop trajectory tracking, already at stop";
+    3169           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3170             :   }
+    3171             : 
+    3172           0 :   return std::tuple(true, ss.str());
+    3173             : }
+    3174             : 
+    3175             : //}
+    3176             : 
+    3177             : /* gotoTrajectoryStartImpl() //{ */
+    3178             : 
+    3179           0 : std::tuple<bool, std::string> MpcTracker::gotoTrajectoryStartImpl(void) {
+    3180             : 
+    3181           0 :   std::stringstream ss;
+    3182             : 
+    3183           0 :   if (trajectory_set_) {
+    3184             : 
+    3185           0 :     toggleHover(false);
+    3186             : 
+    3187           0 :     trajectory_tracking_in_progress_ = false;
+    3188           0 :     timer_trajectory_tracking_.stop();
+    3189             : 
+    3190             :     {
+    3191           0 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3192             : 
+    3193           0 :       setGoal((*des_x_whole_trajectory_)[0], (*des_y_whole_trajectory_)[0], (*des_z_whole_trajectory_)[0], (*des_heading_whole_trajectory_)[0],
+    3194           0 :               trajectory_track_heading_);
+    3195             :     }
+    3196             : 
+    3197           0 :     publishDiagnostics();
+    3198             : 
+    3199           0 :     ss << "flying to the start of the trajectory";
+    3200           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3201             : 
+    3202           0 :     return std::tuple(true, ss.str());
+    3203             : 
+    3204             :   } else {
+    3205             : 
+    3206           0 :     ss << "can not fly to the start of the trajectory, the trajectory is not set";
+    3207           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3208             : 
+    3209           0 :     return std::tuple(false, ss.str());
+    3210             :   }
+    3211             : }
+    3212             : 
+    3213             : //}
+    3214             : 
+    3215             : // | ------------------------- support ------------------------ |
+    3216             : 
+    3217             : /* //{ publishDiagnostics() */
+    3218             : 
+    3219        6444 : void MpcTracker::publishDiagnostics(void) {
+    3220             : 
+    3221       12888 :   auto des_x_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_x_trajectory_);
+    3222       12888 :   auto des_y_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_y_trajectory_);
+    3223       12888 :   auto des_z_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_z_trajectory_);
+    3224       12888 :   auto des_heading_trajectory = mrs_lib::get_mutexed(mutex_des_trajectory_, des_heading_trajectory_);
+    3225             : 
+    3226       12888 :   mrs_msgs::MpcTrackerDiagnostics diagnostics;
+    3227             : 
+    3228        6444 :   diagnostics.header.stamp    = ros::Time::now();
+    3229        6444 :   diagnostics.header.frame_id = uav_state_.header.frame_id;
+    3230             : 
+    3231        6444 :   diagnostics.active = is_active_;
+    3232             : 
+    3233        6444 :   diagnostics.uav_name = _uav_name_;
+    3234             : 
+    3235        6444 :   diagnostics.collision_avoidance_active = collision_avoidance_enabled_;
+    3236        6444 :   diagnostics.avoiding_collision         = collision_avoidance_affecting_me_;
+    3237             : 
+    3238        6444 :   diagnostics.setpoint.position.x = des_x_trajectory(0, 0);
+    3239        6444 :   diagnostics.setpoint.position.y = des_y_trajectory(0, 0);
+    3240        6444 :   diagnostics.setpoint.position.z = des_z_trajectory(0, 0);
+    3241             : 
+    3242        6444 :   diagnostics.setpoint.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    3243             : 
+    3244       12888 :   std::stringstream ss;
+    3245             : 
+    3246             :   {
+    3247       12888 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    3248             : 
+    3249             :     // fill in if other UAVs are sending their trajectories
+    3250        6444 :     std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>::iterator u = other_uav_diagnostics_.begin();
+    3251             : 
+    3252        6444 :     while (u != other_uav_diagnostics_.end()) {
+    3253             : 
+    3254           0 :       if (u->second.collision_avoidance_active) {
+    3255             : 
+    3256             :         // is the other's trajectory fresh enought?
+    3257           0 :         if ((ros::Time::now() - u->second.header.stamp).toSec() < _collision_trajectory_timeout_) {
+    3258           0 :           diagnostics.avoidance_active_uavs.push_back(u->first);
+    3259           0 :           ss << u->first.c_str() << ", ";
+    3260             :         }
+    3261             :       }
+    3262             : 
+    3263           0 :       u++;
+    3264             :     }
+    3265             :   }
+    3266             : 
+    3267       12888 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3268             : 
+    3269        6444 :   if (ss.str().length() > 0) {
+    3270           0 :     ROS_DEBUG_STREAM_THROTTLE(5.0, "[MpcTracker]: getting avoidance trajectories: " << ss.str());
+    3271       14963 :   } else if (collision_avoidance_enabled_ &&
+    3272        8519 :       (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    3273        4581 :     ROS_DEBUG_THROTTLE(10.0, "[MpcTracker]: missing avoidance trajectories!");
+    3274             :   }
+    3275             : 
+    3276        6444 :   pub_diagnostics_.publish(diagnostics);
+    3277             : 
+    3278       12888 :   std_msgs::String string_msg;
+    3279             : 
+    3280        6444 :   if (diagnostics.avoidance_active_uavs.empty()) {
+    3281             : 
+    3282        6444 :     string_msg.data = "-id col_avoid I see: NOTHING";
+    3283             : 
+    3284             :   } else {
+    3285             : 
+    3286           0 :     string_msg.data = "-id col_avoid I see: ";
+    3287             :   }
+    3288             : 
+    3289        6444 :   if (diagnostics.avoidance_active_uavs.size() <= 3) {
+    3290             : 
+    3291        6444 :     for (size_t i = 0; i < diagnostics.avoidance_active_uavs.size(); i++) {
+    3292           0 :       if (i == 0) {
+    3293           0 :         string_msg.data += diagnostics.avoidance_active_uavs[i];
+    3294             :       } else {
+    3295           0 :         string_msg.data += ", " + diagnostics.avoidance_active_uavs[i];
+    3296             :       }
+    3297             :     }
+    3298             : 
+    3299             :   } else {
+    3300             : 
+    3301           0 :     std::stringstream ss;
+    3302           0 :     ss << diagnostics.avoidance_active_uavs.size();
+    3303             : 
+    3304           0 :     string_msg.data += ss.str() + " UAVs";
+    3305             :   }
+    3306             : 
+    3307        6444 :   pub_status_string_.publish(string_msg);
+    3308        6444 : }
+    3309             : 
+    3310             : //}
+    3311             : 
+    3312             : /* debugPrintState() //{ */
+    3313             : 
+    3314         121 : void MpcTracker::debugPrintState(const double throttle) {
+    3315             : 
+    3316         242 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3317             : 
+    3318         121 :   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));
+    3319         121 :   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));
+    3320         121 :   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));
+    3321         121 :   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));
+    3322         121 : }
+    3323             : 
+    3324             : //}
+    3325             : 
+    3326             : /* debugPrintMPCu() //{ */
+    3327             : 
+    3328         121 : void MpcTracker::debugPrintMPCResult(const double throttle) {
+    3329             : 
+    3330         242 :   auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    3331         121 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    3332             : 
+    3333         121 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC result: [%.2f, %.2f, %.2f, %.2f]", mpc_u(0), mpc_u(1), mpc_u(2), mpc_u_heading);
+    3334         121 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: snap constraint: hor: %.2f, ver asc: %.2f, vert desc: %.2f, heading: %.2f]", constraints.horizontal_snap,
+    3335             :                      constraints.vertical_ascending_snap, constraints.vertical_descending_snap, constraints.heading_snap);
+    3336         121 : }
+    3337             : 
+    3338             : //}
+    3339             : 
+    3340             : // --------------------------------------------------------------
+    3341             : // |                           timers                           |
+    3342             : // --------------------------------------------------------------
+    3343             : 
+    3344             : /* //{ timerDiagnostics() */
+    3345             : 
+    3346             : // published diagnostics in reguar intervals
+    3347        6211 : void MpcTracker::timerDiagnostics(const ros::TimerEvent& event) {
+    3348             : 
+    3349        6211 :   if (!is_initialized_)
+    3350           0 :     return;
+    3351             : 
+    3352       18633 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.1, event);
+    3353       18633 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3354             : 
+    3355        6211 :   publishDiagnostics();
+    3356             : }
+    3357             : 
+    3358             : //}
+    3359             : 
+    3360             : /* //{ timerMPC() */
+    3361             : 
+    3362       22051 : void MpcTracker::timerMPC(const ros::TimerEvent& event) {
+    3363             : 
+    3364       22051 :   if (odometry_reset_in_progress_) {
+    3365           0 :     ROS_ERROR("[MpcTracker]: mpc iteration tried run while reseting odometry");
+    3366           0 :     return;
+    3367             :   }
+    3368             : 
+    3369       22051 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3370             : 
+    3371       22051 :   mrs_lib::AtomicScopeFlag unset_running(mpc_timer_running_);
+    3372             : 
+    3373       22051 :   bool started_with_invalid = mpc_result_invalid_;
+    3374             : 
+    3375       22051 :   if (!is_active_) {
+    3376           0 :     return;
+    3377             :   }
+    3378             : 
+    3379       22051 :   if (!is_initialized_) {
+    3380           0 :     return;
+    3381             :   }
+    3382             : 
+    3383       66153 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerMPC", 1.0 / dt1, 0.01, event);
+    3384       66153 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerMPC", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3385             : 
+    3386       22051 :   ros::Time     begin = ros::Time::now();
+    3387       22051 :   ros::Time     end;
+    3388       22051 :   ros::Duration interval;
+    3389             :   int           trajectory_id;
+    3390             : 
+    3391             :   // if we are tracking trajectory, copy the setpoint
+    3392       22051 :   if (trajectory_tracking_in_progress_) {
+    3393             : 
+    3394        1466 :     MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    3395        1466 :     VectorXd des_x_whole_trajectory, des_y_whole_trajectory, des_z_whole_trajectory, des_heading_whole_trajectory;
+    3396             :     double   trajectory_dt;
+    3397             :     int      trajectory_size;
+    3398             :     {
+    3399         733 :       std::scoped_lock lock(mutex_des_trajectory_, mutex_des_whole_trajectory_);
+    3400             : 
+    3401         733 :       des_x_trajectory       = des_x_trajectory_;
+    3402         733 :       des_y_trajectory       = des_y_trajectory_;
+    3403         733 :       des_z_trajectory       = des_z_trajectory_;
+    3404         733 :       des_heading_trajectory = des_heading_trajectory_;
+    3405             : 
+    3406         733 :       des_x_whole_trajectory       = *des_x_whole_trajectory_;
+    3407         733 :       des_y_whole_trajectory       = *des_y_whole_trajectory_;
+    3408         733 :       des_z_whole_trajectory       = *des_z_whole_trajectory_;
+    3409         733 :       des_heading_whole_trajectory = *des_heading_whole_trajectory_;
+    3410             : 
+    3411         733 :       trajectory_size = trajectory_size_;
+    3412         733 :       trajectory_dt   = trajectory_dt_;
+    3413             : 
+    3414         733 :       trajectory_id = des_whole_trajectory_id_;
+    3415             :     }
+    3416             : 
+    3417             :     /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    3418             : 
+    3419         733 :     int trajectory_tracking_sub_idx = trajectory_tracking_sub_idx_;
+    3420         733 :     int trajectory_tracking_idx     = trajectory_tracking_idx_;
+    3421             : 
+    3422       30053 :     for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3423             : 
+    3424       29320 :       double first_time = dt1 + i * _dt2_ + trajectory_tracking_sub_idx * dt1;
+    3425             : 
+    3426       29320 :       int first_idx  = trajectory_tracking_idx + int(floor(first_time / trajectory_dt));
+    3427       29320 :       int second_idx = first_idx + 1;
+    3428             : 
+    3429       29320 :       double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    3430             : 
+    3431       29320 :       if (trajectory_tracking_loop_) {
+    3432             : 
+    3433           0 :         if (second_idx >= trajectory_size) {
+    3434           0 :           second_idx = second_idx % trajectory_size;
+    3435             :         }
+    3436             : 
+    3437           0 :         if (first_idx >= trajectory_size) {
+    3438           0 :           first_idx = first_idx % trajectory_size;
+    3439             :         }
+    3440             : 
+    3441             :       } else {
+    3442             : 
+    3443       29320 :         if (second_idx >= trajectory_size) {
+    3444       14930 :           second_idx = trajectory_size - 1;
+    3445             :         }
+    3446             : 
+    3447       29320 :         if (first_idx >= trajectory_size) {
+    3448       14197 :           first_idx = trajectory_size - 1;
+    3449             :         }
+    3450             :       }
+    3451             : 
+    3452       29320 :       des_x_trajectory(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory[first_idx] + interp_coeff * des_x_whole_trajectory[second_idx];
+    3453       29320 :       des_y_trajectory(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory[first_idx] + interp_coeff * des_y_whole_trajectory[second_idx];
+    3454       29320 :       des_z_trajectory(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory[first_idx] + interp_coeff * des_z_whole_trajectory[second_idx];
+    3455             : 
+    3456       29320 :       des_heading_trajectory(i, 0) = sradians::interp(des_heading_whole_trajectory[first_idx], des_heading_whole_trajectory[second_idx], interp_coeff);
+    3457             :     }
+    3458             : 
+    3459             :     {
+    3460        1466 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3461             : 
+    3462         733 :       des_x_trajectory_       = des_x_trajectory;
+    3463         733 :       des_y_trajectory_       = des_y_trajectory;
+    3464         733 :       des_z_trajectory_       = des_z_trajectory;
+    3465         733 :       des_heading_trajectory_ = des_heading_trajectory;
+    3466             :     }
+    3467             : 
+    3468             :     //}
+    3469             : 
+    3470             :     // increase the trajectory subsampling counter
+    3471             :     {
+    3472         733 :       std::scoped_lock lock(mutex_trajectory_tracking_states_);
+    3473             : 
+    3474         733 :       trajectory_tracking_sub_idx_++;
+    3475             :     }
+    3476             :   } else {
+    3477             : 
+    3478       21318 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3479             : 
+    3480       21318 :     trajectory_id = des_whole_trajectory_id_;
+    3481             :   }
+    3482             : 
+    3483       22051 :   manageConstraints();
+    3484             : 
+    3485       22051 :   calculateMPC();
+    3486             : 
+    3487       22051 :   end      = ros::Time::now();
+    3488       22051 :   interval = end - begin;
+    3489             : 
+    3490             :   // | ------------------ calculate the MPC RTF ----------------- |
+    3491             : 
+    3492       22051 :   mpc_rtf_ = 0.99 * mpc_rtf_ + 0.01 * (interval.toSec()/dt1);
+    3493             : 
+    3494       22051 :   if (mpc_rtf_ >= 1.0) {
+    3495           0 :     ROS_WARN_THROTTLE(5.0, "[MpcTracker] MPC Real Time Factor (%.3f) is slow", mpc_rtf_);
+    3496             :   }
+    3497             : 
+    3498             :   /* publish predicted future //{ */
+    3499             : 
+    3500             :   {
+    3501       44102 :     geometry_msgs::PoseArray debug_trajectory_out;
+    3502       22051 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    3503       22051 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    3504             : 
+    3505             :     {
+    3506       44102 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3507             : 
+    3508      904091 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3509             : 
+    3510      882040 :         geometry_msgs::Pose newPose;
+    3511             : 
+    3512      882040 :         newPose.position.x = predicted_trajectory_(i * _mpc_n_states_);
+    3513      882040 :         newPose.position.y = predicted_trajectory_(i * _mpc_n_states_ + 4);
+    3514      882040 :         newPose.position.z = predicted_trajectory_(i * _mpc_n_states_ + 8);
+    3515             : 
+    3516             :         try {
+    3517      882040 :           newPose.orientation = mrs_lib::AttitudeConverter(0, 0, predicted_heading_trajectory_(i * _mpc_n_states_));
+    3518           0 :         } catch (...) {
+    3519           0 :           ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: failed to fill orientation into debug print trajectory");
+    3520             :         }
+    3521             : 
+    3522      882040 :         debug_trajectory_out.poses.push_back(newPose);
+    3523             :       }
+    3524             :     }
+    3525             : 
+    3526       22051 :     ph_predicted_trajectory_debugging_.publish(debug_trajectory_out);
+    3527             :   }
+    3528             : 
+    3529             :   //}
+    3530             : 
+    3531             :   /* publish full state prediction //{ */
+    3532             : 
+    3533             :   {
+    3534       44102 :     mrs_msgs::MpcPredictionFullState prediction_fs_out;
+    3535       22051 :     prediction_fs_out.header.stamp    = ros::Time::now();
+    3536       22051 :     prediction_fs_out.header.frame_id = uav_state_.header.frame_id;
+    3537             : 
+    3538       22051 :     ros::Time stamp = prediction_fs_out.header.stamp;
+    3539             : 
+    3540       22051 :     prediction_fs_out.input_id = trajectory_id;
+    3541             : 
+    3542             :     {
+    3543       44102 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3544             : 
+    3545      904091 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3546             : 
+    3547      882040 :         if (i == 0) {
+    3548       22051 :           stamp += ros::Duration(0.01);
+    3549             :         } else {
+    3550      859989 :           stamp += ros::Duration(0.2);
+    3551             :         }
+    3552             : 
+    3553      882040 :         prediction_fs_out.stamps.push_back(stamp);
+    3554             : 
+    3555             :         {  // position
+    3556      882040 :           geometry_msgs::Point point;
+    3557             : 
+    3558      882040 :           point.x = predicted_trajectory_(i * _mpc_n_states_);
+    3559      882040 :           point.y = predicted_trajectory_(i * _mpc_n_states_ + 4);
+    3560      882040 :           point.z = predicted_trajectory_(i * _mpc_n_states_ + 8);
+    3561             : 
+    3562      882040 :           prediction_fs_out.position.push_back(point);
+    3563             :         }
+    3564             : 
+    3565             :         {  // velocity
+    3566      882040 :           geometry_msgs::Vector3 vector;
+    3567             : 
+    3568      882040 :           vector.x = predicted_trajectory_(i * _mpc_n_states_ + 1);
+    3569      882040 :           vector.y = predicted_trajectory_(i * _mpc_n_states_ + 5);
+    3570      882040 :           vector.z = predicted_trajectory_(i * _mpc_n_states_ + 9);
+    3571             : 
+    3572      882040 :           prediction_fs_out.velocity.push_back(vector);
+    3573             :         }
+    3574             : 
+    3575             :         {  // acceleration
+    3576      882040 :           geometry_msgs::Vector3 vector3;
+    3577             : 
+    3578      882040 :           vector3.x = predicted_trajectory_(i * _mpc_n_states_ + 2);
+    3579      882040 :           vector3.y = predicted_trajectory_(i * _mpc_n_states_ + 6);
+    3580      882040 :           vector3.z = predicted_trajectory_(i * _mpc_n_states_ + 10);
+    3581             : 
+    3582      882040 :           prediction_fs_out.acceleration.push_back(vector3);
+    3583             :         }
+    3584             : 
+    3585             :         {  // jerk
+    3586      882040 :           geometry_msgs::Vector3 vector3;
+    3587             : 
+    3588      882040 :           vector3.x = predicted_trajectory_(i * _mpc_n_states_ + 3);
+    3589      882040 :           vector3.y = predicted_trajectory_(i * _mpc_n_states_ + 7);
+    3590      882040 :           vector3.z = predicted_trajectory_(i * _mpc_n_states_ + 11);
+    3591             : 
+    3592      882040 :           prediction_fs_out.jerk.push_back(vector3);
+    3593             :         }
+    3594             : 
+    3595             :         {
+    3596             :           // heading
+    3597             : 
+    3598      882040 :           prediction_fs_out.heading.push_back(predicted_heading_trajectory_(i * _mpc_n_states_));
+    3599      882040 :           prediction_fs_out.heading_rate.push_back(predicted_heading_trajectory_(i * _mpc_n_states_ + 1));
+    3600      882040 :           prediction_fs_out.heading_acceleration.push_back(predicted_heading_trajectory_(i * _mpc_n_states_ + 2));
+    3601      882040 :           prediction_fs_out.heading_jerk.push_back(predicted_heading_trajectory_(i * _mpc_n_states_ + 3));
+    3602             :         }
+    3603             :       }
+    3604             :     }
+    3605             : 
+    3606             :     {
+    3607       44102 :       std::scoped_lock lock(mutex_prediction_full_state_);
+    3608       22051 :       prediction_full_state_ = prediction_fs_out;
+    3609             :     }
+    3610             :   }
+    3611             : 
+    3612             :   //}
+    3613             : 
+    3614       22051 :   mpc_computed_ = true;
+    3615             : 
+    3616       22051 :   if (started_with_invalid) {
+    3617             : 
+    3618           0 :     mpc_result_invalid_ = false;
+    3619             : 
+    3620           0 :     ROS_INFO("[MpcTracker]: calculated the first MPC result after invalidation");
+    3621             :   }
+    3622             : }
+    3623             : 
+    3624             : //}
+    3625             : 
+    3626             : /* timerTrajectoryTracking() //{ */
+    3627             : 
+    3628          40 : void MpcTracker::timerTrajectoryTracking(const ros::TimerEvent& event) {
+    3629             : 
+    3630          40 :   auto trajectory_size = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    3631          40 :   auto trajectory_dt   = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_dt_);
+    3632             : 
+    3633         120 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerTrajectoryTracking", int(1.0 / trajectory_dt), 0.01, event);
+    3634             :   mrs_lib::ScopeTimer timer =
+    3635         120 :       mrs_lib::ScopeTimer("MpcTracker::timerTrajectoryTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3636             : 
+    3637             :   {
+    3638          80 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+    3639             : 
+    3640             :     // do a step of the main tracking idx
+    3641             : 
+    3642             :     // reset the subsampling counter
+    3643          40 :     trajectory_tracking_sub_idx_ = 0;
+    3644             : 
+    3645             :     // INCREMENT THE TRACKING IDX
+    3646          40 :     trajectory_tracking_idx_++;
+    3647             : 
+    3648             :     // if the tracking idx hits the end of the trajectory
+    3649          40 :     if (trajectory_tracking_idx_ == trajectory_size) {
+    3650             : 
+    3651           1 :       if (trajectory_tracking_loop_) {
+    3652             : 
+    3653             :         // reset the idx
+    3654           0 :         trajectory_tracking_idx_ = 0;
+    3655             : 
+    3656           0 :         ROS_INFO("[MpcTracker]: trajectory looped");
+    3657             : 
+    3658             :       } else {
+    3659             : 
+    3660           1 :         trajectory_tracking_in_progress_ = false;
+    3661             : 
+    3662             :         // set the idx to the last idx of the trajectory
+    3663           1 :         trajectory_tracking_idx_ = trajectory_size - 1;
+    3664             : 
+    3665           1 :         timer_trajectory_tracking_.stop();
+    3666             : 
+    3667           1 :         ROS_INFO("[MpcTracker]: done tracking trajectory");
+    3668             :       }
+    3669             :     }
+    3670             :   }
+    3671             : 
+    3672          40 :   publishDiagnostics();
+    3673          40 : }
+    3674             : 
+    3675             : //}
+    3676             : 
+    3677             : /* timerVelocityTracking() //{ */
+    3678             : 
+    3679           0 : void MpcTracker::timerVelocityTracking(const ros::TimerEvent& event) {
+    3680             : 
+    3681           0 :   if (!is_initialized_) {
+    3682           0 :     return;
+    3683             :   }
+    3684             : 
+    3685           0 :   if (!velocity_tracking_active_) {
+    3686           0 :     return;
+    3687             :   }
+    3688             : 
+    3689           0 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerVelocityTracking", int(30.0), 0.01, event);
+    3690             :   mrs_lib::ScopeTimer timer =
+    3691           0 :       mrs_lib::ScopeTimer("MpcTracker::timerVelocityTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3692             : 
+    3693             :   // stop the timer when timeout
+    3694           0 :   if ((ros::Time::now() - velocity_reference_time_).toSec() > 0.5) {
+    3695             : 
+    3696           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: velocity reference timeouted, hovering");
+    3697           0 :     timer_velocity_tracking_.stop();
+    3698             : 
+    3699           0 :     toggleHover(true);
+    3700             : 
+    3701           0 :     velocity_tracking_active_ = false;
+    3702             : 
+    3703           0 :     return;
+    3704             :   }
+    3705             : 
+    3706           0 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3707           0 :   auto velocity_reference     = mrs_lib::get_mutexed(mutex_velocity_reference_, velocity_reference_);
+    3708             : 
+    3709           0 :   mrs_msgs::TrajectoryReference trajectory;
+    3710             : 
+    3711           0 :   trajectory.fly_now         = true;
+    3712           0 :   trajectory.use_heading     = true;
+    3713           0 :   trajectory.dt              = 0.2;
+    3714           0 :   trajectory.header.stamp    = ros::Time::now();
+    3715           0 :   trajectory.header.frame_id = "";
+    3716             : 
+    3717           0 :   double x       = mpc_x(0, 0);
+    3718           0 :   double y       = mpc_x(4, 0);
+    3719           0 :   double z       = mpc_x(8, 0);
+    3720           0 :   double heading = mpc_x_heading(0, 0);
+    3721             : 
+    3722           0 :   for (int i = 0; i < 50; i++) {
+    3723             : 
+    3724           0 :     mrs_msgs::Reference reference;
+    3725           0 :     reference.position.x = x;
+    3726           0 :     reference.position.y = y;
+    3727           0 :     reference.position.z = z;
+    3728           0 :     reference.heading    = heading;
+    3729             : 
+    3730           0 :     trajectory.points.push_back(reference);
+    3731             : 
+    3732           0 :     x += velocity_reference.velocity.x * trajectory.dt;
+    3733           0 :     y += velocity_reference.velocity.y * trajectory.dt;
+    3734           0 :     z += velocity_reference.velocity.z * trajectory.dt;
+    3735             : 
+    3736           0 :     if (velocity_reference.use_altitude) {
+    3737           0 :       z = velocity_reference.altitude;
+    3738             :     }
+    3739             : 
+    3740           0 :     if (velocity_reference.use_heading_rate) {
+    3741           0 :       heading += velocity_reference.heading_rate * trajectory.dt;
+    3742           0 :     } else if (velocity_reference.use_heading) {
+    3743           0 :       heading = velocity_reference.heading;
+    3744             :     }
+    3745             :   }
+    3746             : 
+    3747           0 :   auto [success, message, modified] = loadTrajectory(trajectory);
+    3748             : }
+    3749             : 
+    3750             : //}
+    3751             : 
+    3752             : /* //{ timerAvoidanceTrajectory() */
+    3753             : 
+    3754        1224 : void MpcTracker::timerAvoidanceTrajectory(const ros::TimerEvent& event) {
+    3755             : 
+    3756        1224 :   if (!is_active_) {
+    3757         687 :     return;
+    3758             :   }
+    3759             : 
+    3760         578 :   if (!is_initialized_) {
+    3761           0 :     return;
+    3762             :   }
+    3763             : 
+    3764         578 :   if (!sh_estimation_diag_.hasMsg()) {
+    3765           0 :     return;
+    3766             :   } else {
+    3767             :     // we won't try to transform and publish the avoidance prediction if we cannot transform it
+    3768             : 
+    3769         578 :     auto                     estimation_diag      = sh_estimation_diag_.getMsg();
+    3770         578 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    3771             : 
+    3772         578 :     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();
+    3773         578 :     bool got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    3774             : 
+    3775         578 :     if (!got_gps_est && !got_rtk_est) {
+    3776          41 :       return;
+    3777             :     }
+    3778             :   }
+    3779             : 
+    3780        1074 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerAvoidanceTrajectory", _avoidance_trajectory_rate_, 0.1, event);
+    3781             :   mrs_lib::ScopeTimer timer =
+    3782        1074 :       mrs_lib::ScopeTimer("MpcTracker::timerAvoidanceTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3783             : 
+    3784         537 :   auto uav_state            = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3785         537 :   auto predicted_trajectory = mrs_lib::get_mutexed(mutex_predicted_trajectory_, predicted_trajectory_);
+    3786             : 
+    3787         537 :   if (future_was_predicted_) {
+    3788             : 
+    3789         537 :     mrs_msgs::FutureTrajectory avoidance_trajectory;
+    3790             : 
+    3791             :     // fill last trajectory with initial data
+    3792         537 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3793         537 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3794         537 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3795         537 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk");
+    3796         537 :     avoidance_trajectory.points.clear();
+    3797         537 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3798         537 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3799         537 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3800         537 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_;
+    3801             : 
+    3802        1074 :     auto res = common_handlers_->transformer->getTransform(uav_state.header.frame_id, "utm_origin", ros::Time::now());
+    3803             : 
+    3804         537 :     if (!res) {
+    3805             : 
+    3806           0 :       std::string message = "[MpcTracker]: can not transform predicted future to utm_origin";
+    3807           0 :       ROS_WARN_STREAM_ONCE(message);
+    3808           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3809           0 :       return;
+    3810             : 
+    3811             :     } else {
+    3812             : 
+    3813        1074 :       geometry_msgs::TransformStamped tf = res.value();
+    3814             : 
+    3815       22017 :       for (int i = 0; i < _mpc_horizon_len_; i++) {
+    3816             : 
+    3817             :         // original point
+    3818       42960 :         geometry_msgs::PoseStamped original_point;
+    3819             : 
+    3820       21480 :         original_point.header.stamp    = ros::Time::now();
+    3821       21480 :         original_point.header.frame_id = uav_state.header.frame_id;
+    3822             : 
+    3823       21480 :         original_point.pose.position.x = predicted_trajectory(i * _mpc_n_states_);
+    3824       21480 :         original_point.pose.position.y = predicted_trajectory(i * _mpc_n_states_ + 4);
+    3825       21480 :         original_point.pose.position.z = predicted_trajectory(i * _mpc_n_states_ + 8);
+    3826             : 
+    3827       21480 :         original_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    3828             : 
+    3829       42960 :         auto res = common_handlers_->transformer->transform(original_point, tf);
+    3830             : 
+    3831       21480 :         if (res) {
+    3832             : 
+    3833       21480 :           mrs_msgs::FuturePoint new_point;
+    3834             : 
+    3835       21480 :           new_point.x = res.value().pose.position.x;
+    3836       21480 :           new_point.y = res.value().pose.position.y;
+    3837       21480 :           new_point.z = res.value().pose.position.z;
+    3838             : 
+    3839       21480 :           avoidance_trajectory.points.push_back(new_point);
+    3840             : 
+    3841             :         } else {
+    3842             : 
+    3843           0 :           std::string message = "[MpcTracker]: can not transform a point of a future trajectory";
+    3844           0 :           ROS_WARN_STREAM_ONCE(message);
+    3845           0 :           ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3846             :         }
+    3847             :       }
+    3848             :     }
+    3849             : 
+    3850         537 :     ph_avoidance_trajectory_.publish(avoidance_trajectory);
+    3851             :   }
+    3852             : }
+    3853             : 
+    3854             : //}
+    3855             : 
+    3856             : /* timerHover() //{ */
+    3857             : 
+    3858         113 : void MpcTracker::timerHover(const ros::TimerEvent& event) {
+    3859             : 
+    3860         226 :   MatrixXd mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    3861             : 
+    3862         339 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerHover", 10, 0.01, event);
+    3863         339 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerHover", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3864             : 
+    3865         113 :   setRelativeGoal(0, 0, 0, 0, false);
+    3866             : 
+    3867         113 :   if (fabs(mpc_x(1, 0)) < 0.1 && fabs(mpc_x(5, 0)) < 0.1 && fabs(mpc_x(9, 0)) < 0.1) {
+    3868             : 
+    3869          36 :     toggleHover(false);
+    3870             : 
+    3871          36 :     ROS_INFO("[MpcTracker]: timerHover: speed is low, stopping hover timer");
+    3872             :   }
+    3873         113 : }
+    3874             : 
+    3875             : //}
+    3876             : 
+    3877             : }  // namespace mpc_tracker
+    3878             : 
+    3879             : }  // namespace mrs_uav_trackers
+    3880             : 
+    3881             : #include <pluginlib/class_list_macros.h>
+    3882          42 : 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..ebc2596163 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html @@ -0,0 +1,991 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ 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..16f45ed04ce4686695c0da42ad8bb33e4ffb8ae4 GIT binary patch literal 12679 zcmV;2F?i02P)jjk4p?9z%6kqCM5;m#!G6`Ua~8GxoM>-qRbd4~8uG0?TCUmM}EOG`O ziFnp|EKNqBJRt<5Zj4P`*E}lm&#Kd*uIn%>=LTB^y!)N_YQG0YXulmB@QnCJ_je7( zum;YLPA5LrG`0htzzJ}g2y_b|=z5ORZzdY^(K8S_sT%l*+6-950WzO!7Z0{;(oL~z z4%XH+u-bW1XGV8%P2d?T`xQGLxLAw@KaOsU_O9n%=T|pr3S4f|+GT~s*hwz}jQ8}- z-JgFZ?>Z?0PR`qZeq{lV->NVMzye0H6nOI^icB}@O*0r3ZX9D7FcB?byaml%!{`Ae zjCx|GU?Rc2;M^4RF@l3jnLQ%`3$S2uU7lr+F|z06^%Oh|etK}%oqfpAqv5`gK0()! z;(K8XKL|)+*|mG}Y|174&H-ML@G!axpg>5r$HpJ%_`Zpm!d#tx3J&o2*#ml5KsD{U zS&p0rnS!bdq!#DxgP24G`@S8TFIF)lhKQ|QI z34duFW4Z~e0J2(?`5Yg2q1mAdW1OAEQbBQ#Ds3*`ex}vYIwl4qC3|A{V8N($XF#;Y zolXYKEdU;n={ojOy1>O}ot80P@Muo)zjSb&!s!(tW~Q*Y)7W)1Fx_D&IVuoAOxG95 zX1kv6;XLI2Jps8}z^+u!i1^^GnYpen^^3dq+MD(n02!*U*@e`u)&lnM&IHEQ(8j-I zP(+W5#znIk25%r;5LOt7`)+bl$sSWK+J52OHb>u4*z_o1wg?$1nM^vo)`AyP>M{-y z0hYQnJJlKX@CX>5Fkm`DfM?=A>M`iYkf5@&!Q0dcn*{%jE=heNvi;6KJ=ob#?5H(9{&WcBry z9?`>|Ne|?-ot#0T92)g@hyL&~;0;lIjp7p{q8hLM3slE%it!Zi6L)#_XVgj(%V50f z@ExNcBaYn2oV9>jjJdYq0k5(rT-BmkYGBWFZAfK|rMp0)cuJtwuBTuDvfuGzWPlkv z9G&Uozosz~p_mc!hN&RnR9z)6FX<>#zf()s#dC@QRm783} zJG%kiIq!@BNO9(a$G?>UL4aINj7(zsje$YBHX;Xd0<244!f%_dkiy2%X^h*pY_P52 zn+5;j_CReDo4)JEwyxXysJ2@jBmjm%JR%ttz%>H01F?3!Af9n38;FtVFk-ybR8KO7 zf&Ph#4~Q!ORAN*&zi9kkoo8j^<7_Q&oAO5`k=@r1E98R7g-Q)N85IVkxOCizqrx7zeDG{(KyxDO6pn6~Ou-eEb9K1E`;C z>1=znbd3Cn10-dSBW-p6KMeHc_`X3k=chN)#sQwN1G4A`=HOmvR~+@PvZrL_=Rwo9pzCw-WW9PO&@tx2i-1k+;dmVZjZ}v(a;R>`aAA|ZFxSf_EUw7vqsfjJU@CY( z^g*d>I4>e#m}x;>=3Q3-Jw{3YVk3@O19saC=$;f^#tC|M+*`PF$hw;L!o$G%K3efd zn#J#kdW=th)ZD>MPyGIs?te1CW80AcbsN$XopN{o^#zA=2_t9>pahD>)0vn(blpx2 z4_G7S>u?f$1L|yr0d<(+`I-~l&`hhabI=#vXY$1r0tSS^Qyr863agggh9w1HXAd&w z?$_8O9SN88PxBZ%gf$YAOB6M6jD)HcU_AdWVO(I>B|tjY8Q{tTa`q7c3^Wt~8)s$$ zfu0v%e@y9lIK z4JEZ+Gd`3H`}RCdyuHfyAU#QZ{5Sku>!jzMLJ<{Y>9!&Q$`Z61r3b9*;suS#qv>m) zM89<0jd3t{PB#sJP0^sa;JVcTdtc-POI$OB z8d@G!z$($LcX9l>iWnmRSzs8@2?cfe<+jx+W_5$ho7VwLwiH1-09DJ-Nw~t^6T9XB zbzPr@s5EJY%AVh?zuQdHK4BHJ9Z!baJc^CkDC>vR>~}m zn%DOjFwu6W%x~8sdop<_0je0T0#q{mpCqQ1S1ADIkd~cG4QL{UGF=Ur6Z8u;MV3>e z_k8G^*2e>=F0k87_3>RXtpTzp2&8!1+?_pa8~}UAu58R5k_4#7xU3~iV%N*UXKPr~ zi>8=Q49mcgI!IF}r#jt3Mv@OVBrgL^ppuJfW)>G!xUI)G8=aq)nW+KVF*?ENf$_~L zd2h*LdsI1S$Ed!5z-vYhU>Tx&A`?rR;*;*V1{T{8cLgnb9@SZH!w7yOZXa}0&;Uk^ z9mcp(ycaNDvMIQw)@Vs>2X}-N`YeV(1RQEp^t%zS&R;9WvL{Zhd(>SI)IIl3g{*F0 zU&M(j_)86!$O&W7f5p^(IP%JVSI$`^rs75}60>K3?;^%u3TJ-v7-bp8sO|cFckGGb z1()mB;dRyW(Q?P0F)rjxet|ppw^rNH&IWMfMw$ew)njBYx-aB)#-PdU%v8I1yIRG10RQ3&HVyXZ(H7wM_v~I4O}An zu0g_5I`PrLJg^TS9DUeedec#=7-N^Z)?y4`j#b*saE>4nK;+iYcK|PvCn~P)2lmr5_jnN-Q3wFRG^Geh7CT@)V0XgFO{t9|zU0SmVP2C8+gxCiKaEhZx3olA^3(Qin~ zILXXZ5D)kSFyzD}SMK@G_tybojY-U^46jae;A4M4U2Bt`&0Tb9q(>go5CH)J}^Zz$aucb_`kwB!y8vfFEpc{BGr_WtzDSLT4p$?t5V`U4R#k{V_M8E z`8<=7dSg`%M@voAT23FKXuuo}k=U0?3{4kPCdUk00liPG26+5j0#s7{M~s;m&L_YU zBLKD#b2g-?`e1x;Cm;6UqXg`Y@%(KKF#cBAHG5*P#B%jcIT7LjUU6^qkpSib3<|(u z1sG}-Q*L}kuzX*lI~Nc2Kn~xczxP+{Tor?~ZC!heDbScCr_rUQ0zBGvjCNuGId#>% zsR121*DxxH`B6>ra%J0GU5$^>6k308sh$)`Y(Ntz=ct(sc7GE;SIWwIVy4=Iy~^YD z0Bol0?EDnlmMZit@JMMN@4>W z@?l?G{-X(-i?N8277GGgxwYjX$>IutXY@e2^In@Ds>ymz?7YB7A+;;3%!vtaO6RDT z+qYtbz9wN->=?teQGD%Vzi8Ny zdeW@&Ug;4%vdVCNss>DF37F|gDP+PJTL z|33mk*(+ky#P_Jj2o^z^YCJbDmmo?9>;Y9>zwPcL96rL67tXVqUR|;m=T9Z2%h`J- zz-I{YuiPQ&nmH4>oQC}MbD79E-t7D~0+uh>23;5Y@v#RVkwV#JozYUMpocmCq z!*Yt#N_bw10MZ+O*|uMoWZZ)Bd8U`WUOV?}Su8kh!uU*|mRkhm?C{wCZhS+Grn|8M zcL|WarDgy<>{o6esVf&7*0xd}v#CB-0UeJ`dIG5>cE)1pm8ouhFD@%nofJnipcsvi z8N)ATo;~1@oC;n25vBBSQ%JcKX2ua&nZy{lo6YtMZ?7$Yd&Nrm9ELOH{_)stfG=m= z?NJ_Id#Eh)1*jfzmIAdQN`O}ORO+CZ0jqUT220Nz*~C(v*4is8vW3P701NL$j562< zc)4_rOttgMKuqN1SE_QwJe#27BXp%yY_rm8hJa(wT!mwexv)ys=UoNqBPCU+zJFholxW!_34t zB{MHuD@Ln4)*M4>K+72aRe9`isX63MQxr2&3e_-$e_|#J#c;KEUCw@1%%Gh8iCGz= zUML2OSr0SvN5$;I6#j{sEEFTftWyn0E)tN7l^Rg-Lc9Mtgr(xUWNrB~C*y>K$SavmCsyWxzBsS7SXU$f=Kuvj z%NV6p!42#LpAr;1b=?Z3y^wIME%B@G>qX-5(iAphWMmcsS_QY$6Kj&-;u-re!R;ly zg?I~OUo~B6D4xC%j5zEf`CSi#&-4J26Z!ci_|Bc3;S>E{ghn6dzs~N743pr9@-B zY90{kK0GevdeDG4byZr*0m91BI%2>HEq^juyt`@NG#I(d(>n6iB)*@R9cK^05a7qk1xR%Zj|sHe2KZSzF9Hu zskX%zpH*&%=C*WQc6xfi5Ai<+A?$RiS&OgHZ1_OVO&=pPdmoo$#=ed7EtZn zS9d+Opd8oc1c*O(Ct->JP>Inn#(;8y_YdI`;@Snou-!ElmU+dD*RQkEmsXTUs}hw$L~e}#un(Y13KVDU>*Ex&Q`&A~4D|&w&@TYBz$80kN-E%2@ zq7x)iEUnRs%KAnY?==wKcsDyosf~0U(0qPTtVnnM8F&3+0C!g-)HX(Nkt|o!IKg3& zAK~Ea0b@OkuxS*#j+!Dc#@lQ&=$e^dwrdmB(;U@@PfGDWI4O+8kZIY&m%w#n*Eh%P zBFCSY?A=vPVlYO)RND;VWbR~N;ZvQ zcxSkS5EUQQ_Tz&XLh{D0wg4XfQ6pkZP=m#80mJ|*FCOt-F>eTxhe4L}H6<=v%=pwu zdi#mBC08p(oDeExwKk?UB7_;>TXXji*;3aZ7Qz}$QedQ94Pot4EDFfOMtKi^T@kY- zJzu77%JpxK4?oAlzHzu=KzXT#zZy?S%(HC}r4dlu^(_fih2w7o)Hd!-M0urR>wC2V>c^O=Gl^;nCt*}g%{#lcG=Dl5i=YPS$%qlDekkDlT*pT{ zSCMW8qm%@#Y#&0*2nAqM_I{vV*#@%K7C@_NMi{rq*YFDq;M)v#lBbni>&-?kc>Xp$ zY?_!L@-YJqKedz&DhrV5$v<`Bd*nUYqt60*(44nW3)s7+bb2ldx(mOFBhE!@c0m}t z9L|-fKC)K|mv`?Bb{Enw(V{(t;xo>!;Go*YxZgPTtaOjPiM=9b>f3YR=TW@BFxVTFn~kgU7h$kEjl@!-o|@ zlTMoMp!t(C!#Go~&J3e_v@_FtTj+njzWrN=vtM$a@rxikq_iJXvSk%<|DNlD!gBpI zldG3=pRQJZ;+u-A7KO-Nh$*-}2TTB0_VQ&}bBqf^_t{i{97cYe-y>22wyc?&61QxcO6>rsDAwj}zC;G- zNF_uT@a*#f;KebLaTbWd-J1YAj1fp%0r0@n2zYIbblI|ixKsBV3*Z1t3Q*ww33)W| zD*`N45H>7V?YSp^@iRCKUC?`|K=H4JefKM*i?wwRKLwT?D7?)W`$s>ZmI?XcXK@C* zWsHeTu`Hm$O4(3#Jr3gnF|x(Q0k4`PG~{pe)}FMvUMS+7ss}=)$hpJD0d7RYuLN*_ zF}yx#L<}E-;92ee$e53j808o?-P1H+z9>IJ}2hk zCK39Nq%_udd5}U{LrBjMJwKVHAe;dPY{TlNALEDvAj{fCRVa%O0`wI|U4aL~_?7gv7zI$+#URuuWc{RUvnU#TO%fW+jZ1KBAMV|9!= z_NW`?qg=o+j8T=cjHry_5t6Wdz=)}Mpqq>r6SdMf#sL^>Fb>c*GeM>bl_gW5$jUea z;Qow)Di*$15+h{UWn8t4al|Y1v$ul7`(&Y>#&n0gtvJ6Tuk_ zmo7go%ASf0%K!sl|H4zjqz{(;G4nhh-OhfnAhWvg?)v?6x{a6lsAhPuDbAh?Hu*rn z3N2Q1W(Bgym|G1P%T63~%>s3NPg-5f#ZdeN-QxCq>F;69D60ZL@G%bdo+V%Cs2&?; zJEdI47-`#-OO|e%LtP9Iy4w=CdwTg8xyk2RXql@?6{=9eNJF%S9K`7fa=Cb@a7dW^ zpDuuTO3*c5+Cs=yxG%;4$yQ&JQoyG{y#@(13kq^^dc?m)|jlIHJ9!N}dS0)Q)g833Q)T`~SN957ZM)HYn&VPejISbfk;Og(#l z6fu7@^EWepGgBi*zYQxhmN3<|P>wGJ=sU}ITsmK~>?tx218(U$ynHttdJea=65zmq zQ*_`1p~j)>Bs#(y6Fg+v{@!6;EwdOYjN(*3-kE6gMmLT+NAyFoQ{zoRAB>rTSe<-dNE_Vx=aQlfzz$Xl zkw-CN5d#)X;x{LLtt&F;mzYJJ!(HLgbg@iF%c}2j71I$tKfaR{s4H8oY3|;pJ)J$Q$8Mfj6W})>W zr<{qfY!*NhdzQeEWpNJd>}(DJ6CYzuyp)w;+lIl5!&A(o$zL3{97z{2KGFiaj7uT- z@VX?i+(P|FgB;bVPSh^Je;=NBip@P=2AX%(?;%q%r2nN{(ggYnUt#h|z*v1W{s2_@ zx&Jk)``=U~s%4r5su`lcGOBr+f6rY1f2h8Nr!TVxvY`Ay?|!3igU-=C?iH*%&HE!g z_jr85nlMgNwKC^7OlKd&yY%DZ!8mB$TY0VukNfe_B!_sQ{O8R{#JJ2}wjjR8Gg2e9^S88J{Ya zEt)^7-=isdS}9l8%*mhh_BMQa`$N{u|C8#5HIotj8EfW$PIV=Uh1!qh3o*zqIfCt- zH7c*fd`MKQhq1f2hOVK2!-fERmqAgwnhnGZeat&NFc04HzWA7Tr_chN z#$I?(2TScrL*e3I-Wh^$+9=U)Q((};tEb$@y&}ntg#*~!FCgY7s>8>thg?0?sjfwr zc)TR1mFxYOb5?o%GQP!FFDM80u@n*S&7W8N)&)wgUh@?88dKM>gydOo(06+V;vaRR z_E;X1ZrH8|_W?xA3qc)Un67nor=IaH?o_wM+m!c(XsO$SE5^YRqipzLuvA9-g0duq z#?L6ybvc90gXo_>Wq@ZOdIIzsbKU5hpX&o+T>byeRO1J98Q=8xUX((#G`5+lYbTHS zG`0t(#Z7KHD?rA*iJ6-if;@Yb&qG5qJZ@ib2K-}bO?Hpl8wwVwmTE+HY4_?Knf65L zES1K?Me;1ges5_bp6-F1DXd4VMLeMUFMh}~0*N6FTKwG<@AWP=SS9<}39stMyu zF4w9)bB!np2S)E;NEMIl?6g=dn@aNVZ5`K@#?qcb!CpipfwHo~%ZI#C+L+ZLWb`3GoRTGv?s7S6SrtcimF-o=aPe6^fKZmI7y!)|B_dob#UW8O<$HgC>zb*s%B#_M(G8i7xfqG}=+)2rMRbvlu za}@;$1NL~d9%{~cj$XD49uQ{~P3a&1zh;Cu=+m8ysO!u{eGUiQ7&omg;FQTOWG%zi z;9`yxH|XJj9CHHT2l!Ib@v&Dd*~884R!eR+XNaj@Cf8w%wjMqu2(@QB11k5KfRR5; zyuOA4D#r+4Vu*fZUYC1cQ+l{TIxBE*{D1?t|7)uS01oVq3k0xM1I5Um5l$E9VD=}# zFaHFS^E>SRzpvcxiPrE7(d_~2&gKCj)ni`CQ`9#23^%unK@N8`vtK&gG5au(`u3hI znP?r&V3FH541eQOXSi-&VlL?%%eK#wWvHxe(Co)emweP{7!QbG3E&%F5c#>KK;JuU zJAIVKI~cIb**V?D_o}VZp#n6zDrvRsnMVu2D%4|KFgiH2E%eZw+L#dMl|xWp4-h)g zSVt3i*GjXLMM?2>GMCYSn^>ZAF)^I#a8{W&>+o!lV*&MJBzMCiU|y>s=ei4ZQ*Fb~ z%z(tFg>bTY|E*Mbv&CB`6LSK|*tqeh$u{X(7w1e{V*W~Ee8W8#w6dmj{pq&~gG1?| zG<-}{$0&re9End^rc!6b=$w+d9ka54G5Y!9#UdmI5s^v=ZszNK)q1t~DW35_EL@t! zRTvg*g5y@<`BkxC^bwXk20eA*%H z&0^#Jt_r7Doq#`6;gp-15rFnF{#7_7 z+`O?*p3YbBFM`V{x68NKn99zuXiTt>ATNv(u%vTCQL$C2JTkhLRQwd5AiH zN4#N-Tc{JucuL!;F4eqhz*q#-9>hOGrNBq%Q%HD-QfjJDh1zvxg)=_5louDLV#p+k zpQ=~K(_&1C7o9MssJ%{ORJ+5Tsc)RZLk4mwd6&)4p@7Hl*u?4_qYT(P+oc#!2?ovq z$~oeyF)H6qwqbn$FGCTMV#+}+qwWx&RfX0HsbE~=f zQ)Ceb*ty~^({tXQLJf_}RkT$5hbfI!-*u6SMvV0x>9^z!#}ZOsUodPt7EBt#@cNGQ z5edd38_CZ8x3TxaICb9FbsPK3whhNg?!U1|+*dVjMj}^Oi9LN>CFWVi)P(W1r`ap1 zlp(7yVCzE6P$flF{YiEUd%9m2C=YSZo3dv^*Q1qG4?lZcq-b97WcdT6l0E<5(+6%p zruMG;%?KN=6}`)l@9E4Xb_pg!|d{j1?0Z{P~Wmgz*o=NH`JYC9^HyIM^%BRK9^qqN?rpWzh+S1R6AE1ukgWo7CjF$i!aXGmDXSIx&U&+{yv46#bR5hJNm zPsG>ZPk`kr^=-tIs?-}X62*9NjEB9Ts8as~_l2)laNT2fGyv3I1L|5DW3fs-7^54U zG{g~Oymj|qKhk8E%Gvw5#zM`??g^;TXDwqama_+AgoP)$t}SEie0qI6pq0DCO7T>m zjsa9Px(%;H zi!6`;wG1EfR;566vt`c+7+a)vWSB&4j9Oxf6W4}ueXy18*{+wR`jV)ryb-j^-Rc1) zIPI~(VT^9B<2M=8%gV_nxlLHS$@JrxExX;A#pFG2HLr#X~FKvSyD~Buh3I zihMb@zOP`M!e3SvoIA#uFd~OxzjD?&r}5i13P4w~+6Jf^V>hEf~a>3)*z5 z7m`ST4?uIYXFLV37}mHa=fS^L(g8+V_sAyUuRPK{=;n_wCdYZY!DZFc;+0ecWHRTe zHq&+;v_h^05XL0lRS7SQEKzDWA@xX4Z}YI95q|UCr(pX_w2Sf+-%SuN;RrEdTCN`V zQjmXz5Yn|c*+xEwlJk?lNeQ7)q-!*HQV7I5V`tzb7H9fUk|>m%O`yL}dluA+YsGjQ z6Tm_jJ>#t~GyMF|FT@~C_hQ#lU`*o$L}(IV3r3$!6(U41U~k9yh~frSEpd+V;}XBR z;hT@MH=XP$LJfVbm9v22$DY^gaSpgi&UXx$7EW3o(V$lo8|G1UHuc=B7LRH2^xW z^FEnvdqX`^AP*cDS}-0zvJb`NU26dSsE+a?3XJVl+6{=b)zems2BzK z1$3PNY$4{*FR!z>NU11ETS-}VN*F-Vv>p!VWk=rPbd9TKdv$R=vnn!S+V|mrdaAQs zmlL70Kn+o0BGtOC^%|Eg>XwhvTL5|K9?GMeE8ml8Dj3mG>ecOuBAK7DyuaDT|m+O}Gm&crEw*V^y zED(bMK}SMRECF24H{)|K4Ih6UF`HE=n93?yvnr`dl;p#9Hyj>~2iDdLRI2e2;EYd7 z+PU~ru@Yc?Gc-NA(K%1+k*AV{3B6E&p>Oo;?G}^r%2w=yQUmt(W@7059-Cst-F@J7 z(G)URz__9fsXt)r7`appGu0KPvYN-Z)I+741JfMAoiI}N{S2UOjNFX0V7ztLXQ!4d zsr&QcoSm=iE~{pRx*t;6{YP_r&=hJ9xgQkO)UCd0?x&Cn?ioXi7QV9BmSq>HF}lyj z(d9aIRaR@XY25g+#lIPGIr#z*dXwFm?&7PIVkoV*z>Dr?a|k{{c7^fa!WH1nB?(002ovPDHLkV1gZa BEA9XQ literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..8e34fc838e --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:5841114.1 %
Date:2023-12-06 07:59:45Functions: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.1%14.1%
+ 		14.1 %58 / 41136.8 %7 / 19
<unnamed>14.1 %58 / 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..03e8d775b7 --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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.1%14.1%
+ 		14.1 %58 / 41136.8 %7 / 19
<unnamed>14.1 %58 / 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..e1a75dee82 --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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.1%14.1%
+ 		14.1 %58 / 41136.8 %7 / 19
<unnamed>14.1 %58 / 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..f5f8b6b0de --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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.1%14.1%
+ 		14.1 %58 / 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..d7b70c5759 --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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.1%14.1%
+ 		14.1 %58 / 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..6657c0d97f --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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.1%14.1%
+ 		14.1 %58 / 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..d4dc0b1246 --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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&)1
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()6
(anonymous namespace)::ProxyExec0::ProxyExec0()42
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>)42
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)123
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)39318
+
+
+ + + +
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..e7b2659603 --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()42
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()6
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>)42
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&)123
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&)2
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&)1
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&)39318
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..501ed81012 --- /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:5841114.1 %
Date:2023-12-06 07:59:45Functions: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          42 : 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          42 :   this->common_handlers_  = common_handlers;
+     129          42 :   this->private_handlers_ = private_handlers;
+     130             : 
+     131          42 :   _uav_name_ = common_handlers->uav_name;
+     132             : 
+     133          42 :   nh_ = nh;
+     134             : 
+     135          42 :   ros::Time::waitForValid();
+     136             : 
+     137             :   // --------------------------------------------------------------
+     138             :   // |                     loading parameters                     |
+     139             :   // --------------------------------------------------------------
+     140             : 
+     141             :   // | ---------------- load parent's parameters ---------------- |
+     142             : 
+     143          84 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     144             : 
+     145          42 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     146             : 
+     147          42 :   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          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/speed_tracker.yaml");
+     155          42 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/speed_tracker.yaml");
+     156             : 
+     157          84 :   const std::string yaml_prefix = "mrs_uav_trackers/speed_tracker/";
+     158             : 
+     159          42 :   private_handlers->param_loader->loadParam(yaml_prefix + "command_timeout", _external_command_timeout_);
+     160             : 
+     161          42 :   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          42 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "SpeedTracker", _profiler_enabled_);
+     169             : 
+     170             :   // | ----------------------- subscribers ---------------------- |
+     171             : 
+     172          42 :   mrs_lib::SubscribeHandlerOptions shopts;
+     173          42 :   shopts.nh              = nh_;
+     174          42 :   shopts.node_name       = "SpeedTracker";
+     175          42 :   shopts.threadsafe      = true;
+     176          42 :   shopts.autostart       = true;
+     177          42 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     178             : 
+     179          42 :   sh_command_ = mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand>(shopts, "command", &SpeedTracker::callbackCommand, this);
+     180             : 
+     181             :   // | ----------------------- publishers ----------------------- |
+     182             : 
+     183          42 :   ph_rviz_marker_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "rviz_marker", 1);
+     184             : 
+     185             :   // | --------------------- finish the init -------------------- |
+     186             : 
+     187          42 :   is_initialized_ = true;
+     188             : 
+     189          42 :   ROS_INFO("[SpeedTracker]: initialized");
+     190             : 
+     191          42 :   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           6 : void SpeedTracker::deactivate(void) {
+     236             : 
+     237           6 :   is_active_ = false;
+     238             : 
+     239           6 :   ROS_INFO("[SpeedTracker]: deactivated");
+     240           6 : }
+     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       39318 : 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      117954 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     259      117954 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     260             : 
+     261             :   {
+     262       39318 :     std::scoped_lock lock(mutex_uav_state_);
+     263             : 
+     264       39318 :     uav_state_ = uav_state;
+     265             : 
+     266       39318 :     got_uav_state_ = true;
+     267             :   }
+     268             : 
+     269             :   double uav_heading;
+     270             : 
+     271             :   try {
+     272       39318 :     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       39318 :   if (!is_active_) {
+     282       39318 :     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           2 : const std_srvs::SetBoolResponse::ConstPtr SpeedTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     381             : 
+     382           4 :   std_srvs::SetBoolResponse res;
+     383           4 :   std::stringstream         ss;
+     384             : 
+     385           2 :   if (cmd->data != callbacks_enabled_) {
+     386             : 
+     387           0 :     callbacks_enabled_ = cmd->data;
+     388             : 
+     389           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     390           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     391             : 
+     392             :   } else {
+     393             : 
+     394           2 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     395           2 :     ROS_WARN_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     396             :   }
+     397             : 
+     398           2 :   res.message = ss.str();
+     399           2 :   res.success = true;
+     400             : 
+     401           4 :   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         123 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr SpeedTracker::setConstraints([
+     459             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     460             : 
+     461             :   {
+     462         123 :     std::scoped_lock lock(mutex_constraints_);
+     463             : 
+     464         123 :     constraints_ = cmd->constraints;
+     465             :   }
+     466             : 
+     467         246 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     468             : 
+     469         123 :   res.success = true;
+     470         123 :   res.message = "constraints updated";
+     471             : 
+     472         246 :   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           1 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr SpeedTracker::setTrajectoryReference([
+     498             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     499           1 :   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          42 : 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..00d23b1b1130ef11f03376ba0d6e0457010f5a43 GIT binary patch literal 3030 zcmV;{3n}!8P)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#ez=}guRX4yb}3i*hJcrgetyTKP@6|tTv^4NrHmLUe?iYn$TWmyFXlw^kGYKK=t?V zI_2kJ9}#4K4VFNJudAH{SkAVBEc12DE)#8*U-5B^ic~bFp69xq~y0^(ueJIU8@e{ zbk?Pqe`6lsb^4)oOQQt|Kz7%zrRR4LVUl4^Mvb1kDIh`s9!vysDcS>m3=<}|3#vS_ zg~_X~R;M-KN^M99yJqKwE3k3Y`I)XA#jWMOFCZ`6^G6|nCXrsnh;pg-tsk;LKuZC{Rk|h9_*>z0vKMr)I1(nuam|RzbY_t0)WLfGr-As zeB&u(=RB3@{dC^L-4P#g3i-($*GwJ-dYo_J_-vmgD2i&vUdvPi)qdB^6^&B`Wtb|`2MT!H@Gud&+Wl1+>={#17VLAO1_ODs=7@PlI=cLm*EH6{qD(5P4v(mt=uSm? zhAs2A4FcH|(~GQ#3-51>dL+Yj`D}#ZaP%%Vz|&BBsGi zBw2kV(B2@%PRBwp8wuYmATCz1Z%Hv1>h~Y_9fOh7=$= z)Ix83%)PhI&E6)=q<)mV#-nXOk6a$PjVa4IU>AU?#bSaZZnEV-@^0Lag0FhYg&sD+ z?hP~BgyidJY(Z*!_}%=eOnWf*HKt85diWdnkOn9`B^CuGfIm?Wr!GP30R=q%ntC{1 zza9d-R6X3K&F4?Nbk@TOhwEQyKGmv)#8rI%?#aqWLf3OF0{;Hxo`7QJkIQMVJW&rv zvz8S#O=%(zuwD4Tfeym4G$i%!wwJ)*)n9dWr`XDdkba9sba zAS#?+cvcu!I9Ge*F0xxln~-Vi%un{0DscSz+WY`ez~e7jU&k<2Rr^WqH63 zq$mThz7V;?)x+d5zq(g>!l|~@g4mM}1h0TRe-vEuOQ{{ip6T_lF;buTir)0F$+)lR zEjI1XyV@CeL(*Rw{TjD6r@F6IcSMIN4MvocFwDB_LnwQ4e<18?MRi)GX;S@!Lw? zFEzt?Y$8=ZY#ME?3}mXOJRL>?nG}aT{zdU&mY5!~d#Xc#f57hWxQLyN^7r_(=;tX`y$sPgy-k?cQ<0Shn6lY^Yf%F%1q+ihS Y4-n`mY`$4G=Kufz07*qoM6N<$f^yx!`v3p{ 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..4681b295ae --- /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:2023-12-06 07:59:45Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_mav_msgs::quaternionFromYaw(double)40
eth_mav_msgs::yawFromQuaternion(Eigen::Quaternion<double, 0> const&)40
+
+
+ + + +
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..00df7ebbfe --- /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:2023-12-06 07:59:45Functions:22100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_mav_msgs::quaternionFromYaw(double)40
eth_mav_msgs::yawFromQuaternion(Eigen::Quaternion<double, 0> const&)40
+
+
+ + + +
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..6100568762 --- /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:2023-12-06 07:59:45Functions: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          40 : inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
+     131          40 :   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          40 : inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
+     139          40 :   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..a9b91a9bf3 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + 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:2424100.0 %
Date:2023-12-06 07:59:45Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_mav_msgs::EigenTrajectoryPoint::EigenTrajectoryPoint()40
+
+
+ + + +
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..4dfe250483 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html @@ -0,0 +1,84 @@ + + + + + + + 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:2424100.0 %
Date:2023-12-06 07:59:45Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_mav_msgs::EigenTrajectoryPoint::EigenTrajectoryPoint()40
+
+
+ + + +
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..3ee2c96783 --- /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:2424100.0 %
Date:2023-12-06 07:59:45Functions:11100.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          40 :   EigenTrajectoryPoint()
+     192          40 :       : timestamp_ns(-1),
+     193             :         time_from_start_ns(0),
+     194          40 :         position_W(Eigen::Vector3d::Zero()),
+     195          40 :         velocity_W(Eigen::Vector3d::Zero()),
+     196          40 :         acceleration_W(Eigen::Vector3d::Zero()),
+     197          40 :         jerk_W(Eigen::Vector3d::Zero()),
+     198          40 :         snap_W(Eigen::Vector3d::Zero()),
+     199             :         orientation_W_B(Eigen::Quaterniond::Identity()),
+     200          40 :         angular_velocity_W(Eigen::Vector3d::Zero()),
+     201          40 :         angular_acceleration_W(Eigen::Vector3d::Zero()),
+     202          40 :         degrees_of_freedom(MavActuation::DOF4) {
+     203          40 :   }
+     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          40 :   inline double getYaw() const {
+     244          40 :     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          40 :   inline void setFromYaw(double yaw) {
+     254          40 :     orientation_W_B = quaternionFromYaw(yaw);
+     255             :   }
+     256          40 :   inline void setFromYawRate(double yaw_rate) {
+     257          40 :     angular_velocity_W.x() = 0.0;
+     258          40 :     angular_velocity_W.y() = 0.0;
+     259          40 :     angular_velocity_W.z() = yaw_rate;
+     260             :   }
+     261          40 :   inline void setFromYawAcc(double yaw_acc) {
+     262          40 :     angular_acceleration_W.x() = 0.0;
+     263          40 :     angular_acceleration_W.y() = 0.0;
+     264          40 :     angular_acceleration_W.z() = yaw_acc;
+     265          40 :   }
+     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..26d81d99e693319d43cb18eb4ec44b3db73e1fd9 GIT binary patch literal 1483 zcmV;+1vL7JP)gb=KIi{O6P4+ovhDATXcEIumWTt!*g?bXbV=C@( zzUr*a*}`=_7=vAOr0OtmhIQ7%VBr!lPVqK12^j-|Mk!*1*zKm?ivIz+{AtcZ5#lnw zNXDFZ2ZnHA*3m%=ID5Ad%BCnv=kb6udSn5cgAS-gtBfm$$#E)v4DNsRzZokZ20oU1I6F~0{Ad-)y#4qW_?Ed?Np-O{QF%g9|WZt3Bk z!}2XE{YCR?MDgHH%_`s=Je?h`(!$s&0v3o#_@qcQJl4@!^CH2q-v()>HkxVe$UVNb zw}K*jtf}EhT|_iAHNjHJFfH!afd*6qs^B2Zq}W)0VGM7QW|1 zg(;v9?i3gWnpnd!O($=WWlFU=LLT5ztjN`G{)jRHU==!NfmwZ6#}o+#nF62$0`kWK z4CoUoKf^YhAve~c4i!CgwgHo3`^>uKid%nJb`Q{o7JC(Y$0%Af6`&p)ZRszL<+Z{M zNm7yxI zSOF>3&FVhIhyBqx$^^b_1QsH8+W6TfAeIO4tc-c%gS@_=1v)ecr?e?>^fTcNjoH+sCXgzxjc+la(Ib0S6?Y<%a+sZ=RI>|!cO z@g3dFdp**%`!<6D)? zJRWAUc^#;^*qf7isB>vpJ5s&_)bsIW+1|oFh0}bf`xFu2amVRWWMIA)z?R-55G!Dh zF(;!!Ws8J>b;o9MBqf2?M!0)@O$?9Xl8T*emfkow;X@ zuT=PZ6uTVuW2aKGV6O`C?)Xa4t6Ur1Q9;qeFQ~8OV@j3Xx|u_@wy$GsjUrTA+3_bc zDo|%HwF{oJ>%eywc(f#y#o0J@?N7@U++1DQ_J%mV$u-uol~5tWrIPg1)o`#RZPbH~ zkOUbWI$yR-PJ3+DhZ?o7DZdM}RlX!hRheP$hZ@p5T)e&%+J-6%A(_@9)I$8lG9^bT zWRFY~*-9*~) + + + + + + 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:2828100.0 %
Date:2023-12-06 07:59:45Functions: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
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
<unnamed>100.0 %24 / 24100.0 %1 / 1
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-sort-l.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html new file mode 100644 index 0000000000..ec4c7247d8 --- /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:2828100.0 %
Date:2023-12-06 07:59:45Functions: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
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
<unnamed>100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
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..05d0158ca5 --- /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:2828100.0 %
Date:2023-12-06 07:59:45Functions: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
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
<unnamed>100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
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..ef4e566edd --- /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:2828100.0 %
Date:2023-12-06 07:59:45Functions: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
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
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-sort-l.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html new file mode 100644 index 0000000000..da7f8cc283 --- /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:2828100.0 %
Date:2023-12-06 07:59:45Functions: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
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
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..94d38ce71a --- /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:2828100.0 %
Date:2023-12-06 07:59:45Functions: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
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
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..997960532a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + 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:66100.0 %
Date:2023-12-06 07:59:45Functions: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_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..5a9399d670 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html @@ -0,0 +1,80 @@ + + + + + + + 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:66100.0 %
Date:2023-12-06 07:59:45Functions: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_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..4f0ca68e8f --- /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:66100.0 %
Date:2023-12-06 07:59:45Functions:00-
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         128 :   Extremum() : time(0.0), value(0.0), segment_idx(0) {
+      35             :   }
+      36             : 
+      37         104 :   Extremum(double _time, double _value, int _segment_idx) : time(_time), value(_value), segment_idx(_segment_idx) {
+      38             :   }
+      39             : 
+      40         226 :   bool operator<(const Extremum& rhs) const {
+      41         174 :     return value < rhs.value;
+      42             :   }
+      43          18 :   bool operator>(const Extremum& rhs) const {
+      44          18 :     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..42a2d6ec4d0c683918ea2dd366e04e84d905a8f2 GIT binary patch literal 437 zcmV;m0ZRUfP)}djTrb8O;8fO7I^n`rmt^V#vdcju9D$0wQ)BB=PsMmKbbvD+ zO+8TU@*;2r^g!(gLp&@qoQi^i_|%RGE>V?7>&#QsRv~@rPQC=*?)=ugoIVLY44k2+ zet&Rs@raS|UY9YnV-+uodfdC(AyB-$9z5!EEw~LQGc0x7vMoy<*R74|x45?u*EnRO z5mss=Tx?n^`^q`T^*_|h))>;TX3nd}7!5*!?x}}k{37+ZSwZGE!M<5(Yp_=7ta<|B zI60Sl!dU0S9(eRza1&9^ + + + + + + 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:29162546.6 %
Date:2023-12-06 07:59:45Functions:193259.4 %
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 +
29.8%29.8%
+ 		29.8 %123 / 41344.4 %8 / 18
<unnamed>29.8 %123 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
<unnamed>79.2 %168 / 21278.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..5b09ee4af4 --- /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:29162546.6 %
Date:2023-12-06 07:59:45Functions:193259.4 %
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 +
29.8%29.8%
+ 		29.8 %123 / 41344.4 %8 / 18
<unnamed>29.8 %123 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
<unnamed>79.2 %168 / 21278.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..9e53d81b34 --- /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:29162546.6 %
Date:2023-12-06 07:59:45Functions:193259.4 %
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.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
<unnamed>79.2 %168 / 21278.6 %11 / 14
polynomial_optimization_nonlinear_impl.h +
29.8%29.8%
+ 		29.8 %123 / 41344.4 %8 / 18
<unnamed>29.8 %123 / 41344.4 %8 / 18
+
+
+ + + + +
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..23d45aa0ef --- /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:29162546.6 %
Date:2023-12-06 07:59:45Functions:193259.4 %
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 +
29.8%29.8%
+ 		29.8 %123 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.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..d4261183e7 --- /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:29162546.6 %
Date:2023-12-06 07:59:45Functions:193259.4 %
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 +
29.8%29.8%
+ 		29.8 %123 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.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..4b1fa125a9 --- /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:29162546.6 %
Date:2023-12-06 07:59:45Functions:193259.4 %
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.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
polynomial_optimization_nonlinear_impl.h +
29.8%29.8%
+ 		29.8 %123 / 41344.4 %8 / 18
+
+
+ + + + +
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..af98b7db1b --- /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:16821279.2 %
Date:2023-12-06 07:59:45Functions: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)1
eth_trajectory_generation::PolynomialOptimization<10>::setupConstraintReorderingMatrix()1
eth_trajectory_generation::PolynomialOptimization<10>::PolynomialOptimization(unsigned long)1
eth_trajectory_generation::PolynomialOptimization<10>::computeCost() const1
eth_trajectory_generation::PolynomialOptimization<10>::solveLinear()2
eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentsFromCompactConstraints()2
eth_trajectory_generation::PolynomialOptimization<10>::constructR(Eigen::SparseMatrix<double, 0, int>*) const2
eth_trajectory_generation::PolynomialOptimization<10>::setupMappingMatrix(double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)3
eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentTimes(std::vector<double, std::allocator<double> > const&)3
eth_trajectory_generation::PolynomialOptimization<10>::invertMappingMatrix(Eigen::Matrix<double, 10, 10, 0, 10, 10> const&, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)3
eth_trajectory_generation::PolynomialOptimization<10>::computeQuadraticCostJacobian(int, double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)3
+
+
+ + + +
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..fd432e5082 --- /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:16821279.2 %
Date:2023-12-06 07:59:45Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimization<10>::solveLinear()2
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)1
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>*)3
eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentTimes(std::vector<double, std::allocator<double> > const&)3
eth_trajectory_generation::PolynomialOptimization<10>::invertMappingMatrix(Eigen::Matrix<double, 10, 10, 0, 10, 10> const&, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)3
eth_trajectory_generation::PolynomialOptimization<10>::computeQuadraticCostJacobian(int, double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)3
eth_trajectory_generation::PolynomialOptimization<10>::setupConstraintReorderingMatrix()1
eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentsFromCompactConstraints()2
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)1
eth_trajectory_generation::PolynomialOptimization<10>::constructR(Eigen::SparseMatrix<double, 0, int>*) const2
eth_trajectory_generation::PolynomialOptimization<10>::computeCost() const1
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..42cae76355 --- /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:16821279.2 %
Date:2023-12-06 07:59:45Functions: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           1 : 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           1 :       n_free_constraints_(0) {
+      53           1 :   fixed_constraints_compact_.resize(dimension_);
+      54           1 :   free_constraints_compact_.resize(dimension_);
+      55           1 : }
+      56             : 
+      57             : //}
+      58             : 
+      59             : /* setupFromVertices() //{ */
+      60             : 
+      61             : template <int _N>
+      62           1 : bool PolynomialOptimization<_N>::setupFromVertices(const Vertex::Vector& vertices, const std::vector<double>& times, int derivative_to_optimize) {
+      63           1 :   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           1 :   derivative_to_optimize_ = derivative_to_optimize;
+      69           1 :   vertices_               = vertices;
+      70           1 :   segment_times_          = times;
+      71             : 
+      72           1 :   n_vertices_ = vertices.size();
+      73           1 :   n_segments_ = n_vertices_ - 1;
+      74             : 
+      75           1 :   segments_.resize(n_segments_, Segment(N, dimension_));
+      76             : 
+      77           1 :   CHECK(n_vertices_ == times.size() + 1) << "Size of times must be one less than positions.";
+      78             : 
+      79           1 :   inverse_mapping_matrices_.resize(n_segments_);
+      80           1 :   cost_matrices_.resize(n_segments_);
+      81             : 
+      82             :   // Iterate through all vertices and remove invalid constraints (order too
+      83             :   // high).
+      84           3 :   for (size_t vertex_idx = 0; vertex_idx < n_vertices_; ++vertex_idx) {
+      85           2 :     Vertex& vertex = vertices_[vertex_idx];
+      86             : 
+      87             :     // Check if we have valid constraints.
+      88           2 :     bool   vertex_valid = true;
+      89           4 :     Vertex vertex_tmp(dimension_);
+      90           9 :     for (Vertex::Constraints::const_iterator it = vertex.cBegin(); it != vertex.cEnd(); ++it) {
+      91           7 :       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           7 :         vertex_tmp.addConstraint(it->first, it->second);
+      97             :       }
+      98             :     }
+      99           2 :     if (!vertex_valid) {
+     100           2 :       vertex = vertex_tmp;
+     101             :     }
+     102             :   }
+     103           1 :   updateSegmentTimes(times);
+     104           1 :   setupConstraintReorderingMatrix();
+     105           1 :   return true;
+     106             : }
+     107             : 
+     108             : //}
+     109             : 
+     110             : /* setupMappingMatrix() //{ */
+     111             : 
+     112             : template <int _N>
+     113           3 : 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          18 :   for (int i = 0; i < N / 2; ++i) {
+     118          15 :     A->row(i)         = Polynomial::baseCoeffsWithTime(N, i, 0.0);
+     119          15 :     A->row(i + N / 2) = Polynomial::baseCoeffsWithTime(N, i, segment_time);
+     120             :   }
+     121           3 : }
+     122             : 
+     123             : //}
+     124             : 
+     125             : /* computeCost() //{ */
+     126             : 
+     127             : template <int _N>
+     128           1 : double PolynomialOptimization<_N>::computeCost() const {
+     129           1 :   CHECK(n_segments_ == segments_.size() && n_segments_ == cost_matrices_.size());
+     130             :   double cost = 0;
+     131           2 :   for (size_t segment_idx = 0; segment_idx < n_segments_; ++segment_idx) {
+     132           1 :     const SquareMatrix& Q       = cost_matrices_[segment_idx];
+     133           1 :     const Segment&      segment = segments_[segment_idx];
+     134           5 :     for (size_t dimension_idx = 0; dimension_idx < dimension_; ++dimension_idx) {
+     135           8 :       const Eigen::VectorXd c            = segment[dimension_idx].getCoefficients(derivative_order::POSITION);
+     136           4 :       const double          partial_cost = c.transpose() * Q * c;
+     137           4 :       cost += partial_cost;
+     138             :     }
+     139             :   }
+     140           1 :   return 0.5 * cost;  // cost = 0.5 * c^T * Q * c
+     141             : }
+     142             : 
+     143             : //}
+     144             : 
+     145             : /* invertMappingMatrix() //{ */
+     146             : 
+     147             : template <int _N>
+     148           3 : 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           3 :   const int half_n = N / 2;
+     163             : 
+     164             :   // "template" keyword required below as half_n is dependent on the template
+     165             :   // parameter.
+     166           3 :   const Eigen::Matrix<double, half_n, 1>      A_diag = mapping_matrix.template block<half_n, half_n>(0, 0).diagonal();
+     167           3 :   const Eigen::Matrix<double, half_n, half_n> A_inv  = A_diag.cwiseInverse().asDiagonal();
+     168             : 
+     169           3 :   const Eigen::Matrix<double, half_n, half_n> C = mapping_matrix.template block<half_n, half_n>(half_n, 0);
+     170             : 
+     171           3 :   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           3 :   inverse_mapping_matrix->template block<half_n, half_n>(0, 0) = A_inv;
+     174           3 :   inverse_mapping_matrix->template block<half_n, half_n>(0, half_n).setZero();
+     175           3 :   inverse_mapping_matrix->template block<half_n, half_n>(half_n, 0)      = -D_inv * C * A_inv;
+     176           3 :   inverse_mapping_matrix->template block<half_n, half_n>(half_n, half_n) = D_inv;
+     177           3 : }
+     178             : 
+     179             : //}
+     180             : 
+     181             : /* setupConstraintReorderingMatrix() //{ */
+     182             : 
+     183             : template <int _N>
+     184           1 : void PolynomialOptimization<_N>::setupConstraintReorderingMatrix() {
+     185             :   typedef Eigen::Triplet<double> Triplet;
+     186           2 :   std::vector<Triplet>           reordering_list;
+     187             : 
+     188           1 :   const size_t n_vertices = vertices_.size();
+     189             : 
+     190           2 :   std::vector<Constraint> all_constraints;
+     191           1 :   std::set<Constraint>    fixed_constraints;
+     192           1 :   std::set<Constraint>    free_constraints;
+     193             : 
+     194           1 :   all_constraints.reserve(n_vertices_ * N / 2);  // Will have exactly this number of elements in the end.
+     195             : 
+     196           3 :   for (size_t vertex_idx = 0; vertex_idx < n_vertices; ++vertex_idx) {
+     197           2 :     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           2 :     int n_constraint_occurence = 2;
+     203           2 :     if (vertex_idx == 0 || vertex_idx == (n_segments_))
+     204           2 :       n_constraint_occurence = 1;
+     205           4 :     for (int co = 0; co < n_constraint_occurence; ++co) {
+     206          12 :       for (size_t constraint_idx = 0; constraint_idx < N / 2; ++constraint_idx) {
+     207          10 :         Constraint constraint;
+     208          10 :         constraint.vertex_idx     = vertex_idx;
+     209          10 :         constraint.constraint_idx = constraint_idx;
+     210          10 :         bool has_constraint       = vertex.getConstraint(constraint_idx, &(constraint.value));
+     211          10 :         if (has_constraint) {
+     212           7 :           all_constraints.push_back(constraint);
+     213          10 :           fixed_constraints.insert(constraint);
+     214             :         } else {
+     215           3 :           constraint.value = Vertex::ConstraintValue::Constant(dimension_, 0);
+     216           3 :           all_constraints.push_back(constraint);
+     217          10 :           free_constraints.insert(constraint);
+     218             :         }
+     219             :       }
+     220             :     }
+     221             :   }
+     222             : 
+     223           1 :   n_all_constraints_   = all_constraints.size();
+     224           1 :   n_fixed_constraints_ = fixed_constraints.size();
+     225           1 :   n_free_constraints_  = free_constraints.size();
+     226             : 
+     227           1 :   reordering_list.reserve(n_all_constraints_);
+     228           1 :   constraint_reordering_ = Eigen::SparseMatrix<double>(n_all_constraints_, n_fixed_constraints_ + n_free_constraints_);
+     229             : 
+     230           5 :   for (Eigen::VectorXd& df : fixed_constraints_compact_)
+     231           8 :     df.resize(n_fixed_constraints_, Eigen::NoChange);
+     232             : 
+     233           1 :   int row = 0;
+     234           1 :   int col = 0;
+     235          11 :   for (const Constraint& ca : all_constraints) {
+     236          80 :     for (const Constraint& cf : fixed_constraints) {
+     237          70 :       if (ca == cf) {
+     238           7 :         reordering_list.emplace_back(Triplet(row, col, 1.0));
+     239          35 :         for (size_t d = 0; d < dimension_; ++d) {
+     240          28 :           Eigen::VectorXd&      df                        = fixed_constraints_compact_[d];
+     241          56 :           const Eigen::VectorXd constraint_all_dimensions = cf.value;
+     242          28 :           df[col]                                         = constraint_all_dimensions[d];
+     243             :         }
+     244             :       }
+     245          70 :       ++col;
+     246             :     }
+     247          40 :     for (const Constraint& cp : free_constraints) {
+     248          30 :       if (ca == cp)
+     249           3 :         reordering_list.emplace_back(Triplet(row, col, 1.0));
+     250          30 :       ++col;
+     251             :     }
+     252          10 :     col = 0;
+     253          10 :     ++row;
+     254             :   }
+     255             : 
+     256           1 :   constraint_reordering_.setFromTriplets(reordering_list.begin(), reordering_list.end());
+     257           1 : }
+     258             : 
+     259             : //}
+     260             : 
+     261             : /* setupConstraintReorderingMatrix() //{ */
+     262             : 
+     263             : template <int _N>
+     264           2 : void PolynomialOptimization<_N>::updateSegmentsFromCompactConstraints() {
+     265           2 :   const size_t n_all_constraints = n_fixed_constraints_ + n_free_constraints_;
+     266             : 
+     267          10 :   for (size_t dimension_idx = 0; dimension_idx < dimension_; ++dimension_idx) {
+     268           8 :     const Eigen::VectorXd& df     = fixed_constraints_compact_[dimension_idx];
+     269           8 :     const Eigen::VectorXd& dp_opt = free_constraints_compact_[dimension_idx];
+     270             : 
+     271          16 :     Eigen::VectorXd d_all(n_all_constraints);
+     272           8 :     d_all << df, dp_opt;
+     273             : 
+     274          16 :     for (size_t i = 0; i < n_segments_; ++i) {
+     275           8 :       const Eigen::Matrix<double, N, 1> new_d   = constraint_reordering_.block(i * N, 0, N, n_all_constraints) * d_all;
+     276           8 :       const Eigen::Matrix<double, N, 1> coeffs  = inverse_mapping_matrices_[i] * new_d;
+     277           8 :       Segment&                          segment = segments_[i];
+     278           8 :       segment.setTime(segment_times_[i]);
+     279           8 :       segment[dimension_idx] = Polynomial(N, coeffs);
+     280             :     }
+     281             :   }
+     282           2 : }
+     283             : 
+     284             : //}
+     285             : 
+     286             : /* updateSegmentTimes() //{ */
+     287             : 
+     288             : template <int _N>
+     289           3 : void PolynomialOptimization<_N>::updateSegmentTimes(const std::vector<double>& segment_times) {
+     290           3 :   const size_t n_segment_times = segment_times.size();
+     291           3 :   CHECK(n_segment_times == n_segments_) << "Number of segment times (" << n_segment_times << ") does not match number of segments (" << n_segments_ << ")";
+     292             : 
+     293           3 :   segment_times_ = segment_times;
+     294             : 
+     295           6 :   for (size_t i = 0; i < n_segments_; i++) {
+     296           3 :     const double segment_time = segment_times[i];
+     297           3 :     CHECK_GT(segment_time, 0) << "Segment times need to be greater than zero";
+     298             : 
+     299           3 :     computeQuadraticCostJacobian(derivative_to_optimize_, segment_time, &cost_matrices_[i]);
+     300           3 :     SquareMatrix A;
+     301           3 :     setupMappingMatrix(segment_time, &A);
+     302           3 :     invertMappingMatrix(A, &inverse_mapping_matrices_[i]);
+     303             :   };
+     304           3 : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* constructR() //{ */
+     309             : 
+     310             : template <int _N>
+     311           2 : void PolynomialOptimization<_N>::constructR(Eigen::SparseMatrix<double>* R) const {
+     312           2 :   CHECK_NOTNULL(R);
+     313             :   typedef Eigen::Triplet<double> Triplet;
+     314           4 :   std::vector<Triplet>           cost_unconstrained_triplets;
+     315           2 :   cost_unconstrained_triplets.reserve(N * N * n_segments_);
+     316             : 
+     317           4 :   for (size_t i = 0; i < n_segments_; ++i) {
+     318           2 :     const SquareMatrix& Ai        = inverse_mapping_matrices_[i];
+     319           2 :     const SquareMatrix& Q         = cost_matrices_[i];
+     320           2 :     const SquareMatrix  H         = Ai.transpose() * Q * Ai;
+     321           2 :     const int           start_pos = i * N;
+     322          22 :     for (int row = 0; row < N; ++row) {
+     323         220 :       for (int col = 0; col < N; ++col) {
+     324         200 :         cost_unconstrained_triplets.emplace_back(Triplet(start_pos + row, start_pos + col, H(row, col)));
+     325             :       }
+     326             :     }
+     327             :   }
+     328           4 :   Eigen::SparseMatrix<double> cost_unconstrained(N * n_segments_, N * n_segments_);
+     329           2 :   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           2 :   *R = constraint_reordering_.transpose() * cost_unconstrained * constraint_reordering_;
+     334           2 : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* solveLinear() //{ */
+     339             : 
+     340             : template <int _N>
+     341           2 : bool PolynomialOptimization<_N>::solveLinear() {
+     342           2 :   CHECK(derivative_to_optimize_ >= 0 && derivative_to_optimize_ <= kHighestDerivativeToOptimize);
+     343             :   // Catch the fully constrained case:
+     344           2 :   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           4 :   Eigen::SparseMatrix<double> R;
+     357           2 :   constructR(&R);
+     358             : 
+     359             :   // Extract block matrices and prepare solver.
+     360           4 :   Eigen::SparseMatrix<double> Rpf = R.block(n_fixed_constraints_, 0, n_free_constraints_, n_fixed_constraints_);
+     361           4 :   Eigen::SparseMatrix<double> Rpp = R.block(n_fixed_constraints_, n_fixed_constraints_, n_free_constraints_, n_free_constraints_);
+     362           4 :   Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver;
+     363          10 :   solver.compute(Rpp);
+     364             : 
+     365             :   // Compute dp_opt for every dimension.
+     366          10 :   for (size_t dimension_idx = 0; dimension_idx < dimension_; ++dimension_idx) {
+     367          16 :     Eigen::VectorXd df                       = -Rpf * fixed_constraints_compact_[dimension_idx];  // Rpf = Rfp^T
+     368           8 :     free_constraints_compact_[dimension_idx] = solver.solve(df);                                  // dp = -Rpp^-1 * Rpf * df
+     369             :   }
+     370             : 
+     371           2 :   updateSegmentsFromCompactConstraints();
+     372             :   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           3 : void PolynomialOptimization<_N>::computeQuadraticCostJacobian(int derivative, double t, SquareMatrix* cost_jacobian) {
+     607           3 :   CHECK_LT(derivative, N);
+     608             : 
+     609           3 :   cost_jacobian->setZero();
+     610          27 :   for (int col = 0; col < N - derivative; col++) {
+     611         216 :     for (int row = 0; row < N - derivative; row++) {
+     612         192 :       double exponent = (N - 1 - derivative) * 2 + 1 - row - col;
+     613             : 
+     614         192 :       (*cost_jacobian)(N - 1 - row, N - 1 - col) =
+     615         192 :           Polynomial::base_coefficients_(derivative, N - 1 - row) * Polynomial::base_coefficients_(derivative, N - 1 - col) * pow(t, exponent) * 2.0 / exponent;
+     616             :     }
+     617             :   }
+     618           3 : }
+     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..d2bf04eef3eaca36f74c3ea7739ee1959089d779 GIT binary patch literal 2760 zcmV;(3ODtMP)WD5Wqft1nR4vXzhq1jr=v56I3|EayI~Edm_zG zxKAju{tX%$*)wXRRy<5m0n|}cm#R`|n$o41Xl{!MmyfL_>4|CB#yN@`P6H$0-YG8p ze+y-~-JE!IYZj_qHSE>aU=}#m1f9-2J0yB-+r7T+^`Zs30$6<0tXd14vbw4w6xUm{ zD=6v}*ESW`ifhhr>zw>$yjkw|`{Q_=kLTax;r~D30~UW1ZV~wW3{Q{vC|sTt4u3g4 z@D5-2XXp8dpMm5Zik>~PGfoc3Oj`g*U%IM>rTWOgx7F;HwN3|oUfRI9s)cZcHoQrZ z6veBL!&@6J{{Q{uHgZt_TO!m}z3_^J{7l35IHBUmBQIpEk1q1r(p513$uDdper0X9 zKNFn+U&??3W!je9SAApQHJHe4lvbq!lw%KzjVf;)+MXxUQpJfR(^dpa0cIwcj(e*} z;0YE7O%q$IGdx-1Zw@d|N3cK#{`4SQy3n`%=Wly*CGA z)ltX#K;8VRclT1>^r|kH*a6yZHxQ_H71ZUUuEHfA?nSNwJwjK(TXQR>4U0qBe+Skc z_}E{5XWd>`vEFa^!oi~0rwR4NSJjXy7%4Dfg$;5-T@Yvk%_*f8mO~Sv&gZIa(>9z+t8{TNGSYu#i;!@(|pmm`IKo6H!l2P zoy+<7G`CR3-TqZT<&?TXpxX>ZvxQ2{cu0g$=F0KQZR1FH)~IbwH%*q-cmtsCHZM!6 zz}t?hAk9ME0BA=c?_h8+mbL*zw=h1dashWTkZjki)V;R|*m zUS>EdZnhRLD}POKd!eqhGkY6Z2fh^|sVvw1=y$Z52`#sC2A~=5q0vKajP{`y@|lhI zaHIV7)VL5N0YV))q3*oA=vTuUa!H`LUp#1FpuYi?E3a7t3A?wkGUu15?-Ot?8py z`oNY>ykc>o{W;03Gg&o_X>P9SaqwXlxC;|W3@L#PWDdm$yLCM)LBOgMVFq*=kQ`;p zlUup8ze$q*qkJkX8v$LcQB*JX=9pr&D>=n$4FFWEps0?ev_zOQQiyO!Q7^&|bVi}z zI0{u1wNcj>3~9ZP1pd}D{W4t7pbjUCilF3H03vwO;woaPPR4OzMO*Qtuxbe`HzJYZ zOdCnhQae@$It<$lDC(=u4Rh61OJmIe753~4E;WFA4z%R3C)Mncoi44qU~j$GvIY@+ zDCX;{qt{<%0RBt+g+eiGZUCfrmhmysFmbgMKZ8&69;p?psrN*v99z+(#FUD96H<>9 zQz9IY#oog<&kMGvEtzm(U@t1%plDmeHNm7M<)gAQUo;9Y8i9f0h0|~tG-5k-#K%3I zw5H>ac{Mec%ei7s_~AngSZD&Q_xI|IS15Q_qDroKv} z;MEY>TmnRyTHeyQI0xy)L?D;64cQg0Lx-JyKDT{kjNjceh7P8{05)Pi4a0Edfrz2b zR%gmEhFp$w_Uso~v!6*0spp22?8)VClbjG<6NDXc@UD4v&|tWzI)ss145!RO$QQ&E zWtrTJSjQsQYEE*$pR7l+O z-h=8GQNQXHvvAj_p0o$OCBQJW<1yWp{8#x#;@L4<8WBwIyh z)^zz*B7DfxTg`IHqY|AG+!_AW$V+*I^B8cR<2d3uvV+cE91j30OFR!u139MHLGgn= za=bqCCWSjAxm5Mda)$ATvjp~7P6t*Nr4KjZ#QiJ9eMfsgZ+kl63Bx39Om>Y2#8R4= zJ2S4i+Z#%@oH@EX)^knaB%rt~rs;A`oNRD#%kL{~1WKvPP-$)rSViVQ!^`_u89<_y zea5hXF@{|EST&>$cPY|ufznDxUxKkJ>ek455nWXSx2<;|kVnmBtW1WX$p@vRDLI)+ z+oXm;5b@Yf{&!N@X2|)2_w~GJT-AEY6G+*SNm!PKusmE9S*TZRwpPLG1z(DV~Bi&13iB6(f&$z@K=;YcKkQ?;?4? zdjmRe4&WCD3~={IaiEdnRRpqBfw~1>WcU!M-UVO>U^Nsrsg{)8si@`6qGLCAW{A7L zW&qmko-+WdVSWF$75l%=80wk&Q+zICDLA|$T(Xic!d0P%D#cs^DvlOft-3sJm!%Ism9t`i8@81p`93P)A7;E9d^F zRUq}>r)HvyZ!4Ty%|_|-c$h!QiL<>#trt&AcVEjRlfLOauzQ&mn + + + + + + 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:12341329.8 %
Date:2023-12-06 07:59:45Functions:81844.4 %
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>::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) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#3}::operator()(double) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#1}::operator()(double) const [clone .isra.0]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)1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getCostAndGradientMellinger(std::vector<double, std::allocator<double> >*)1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::scaleSegmentTimesWithViolation()1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeMellingerOuterLoop(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimize()1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::PolynomialOptimizationNonLinear(unsigned long, eth_trajectory_generation::NonlinearOptimizationParameters const&)1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::addMaximumMagnitudeConstraint(int, int, double)12
+
+
+ + + +
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..07c0518167 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html @@ -0,0 +1,152 @@ + + + + + + + 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:12341329.8 %
Date:2023-12-06 07:59:45Functions:81844.4 %
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)1
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>::getCostAndGradientMellinger(std::vector<double, std::allocator<double> >*)1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::addMaximumMagnitudeConstraint(int, int, double)12
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeAndFreeConstraints()0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::scaleSegmentTimesWithViolation()1
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*)1
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()1
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::PolynomialOptimizationNonLinear(unsigned long, eth_trajectory_generation::NonlinearOptimizationParameters const&)1
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) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#3}::operator()(double) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#1}::operator()(double) const [clone .isra.0]0
+
+
+ + + +
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..905b818f72 --- /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:12341329.8 %
Date:2023-12-06 07:59:45Functions:81844.4 %
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           1 : PolynomialOptimizationNonLinear<_N>::PolynomialOptimizationNonLinear(size_t dimension, const NonlinearOptimizationParameters& parameters)
+      48           1 :     : poly_opt_(dimension), optimization_parameters_(parameters) {
+      49           1 : }
+      50             : 
+      51             : template <int _N>
+      52           1 : bool PolynomialOptimizationNonLinear<_N>::setupFromVertices(const Vertex::Vector& vertices, const std::vector<double>& segment_times,
+      53             :                                                             int derivative_to_optimize) {
+      54           1 :   bool ret = poly_opt_.setupFromVertices(vertices, segment_times, derivative_to_optimize);
+      55             : 
+      56             :   size_t n_optimization_parameters;
+      57           1 :   switch (optimization_parameters_.time_alloc_method) {
+      58           1 :     case NonlinearOptimizationParameters::kSquaredTime:
+      59             :     case NonlinearOptimizationParameters::kRichterTime:
+      60             :     case NonlinearOptimizationParameters::kMellingerOuterLoop:
+      61           1 :       n_optimization_parameters = segment_times.size();
+      62           1 :       break;
+      63           0 :     default:
+      64           0 :       n_optimization_parameters = segment_times.size() + poly_opt_.getNumberFreeConstraints() * poly_opt_.getDimension();
+      65           0 :       break;
+      66             :   }
+      67             : 
+      68           1 :   nlopt_.reset(new nlopt::opt(optimization_parameters_.algorithm, n_optimization_parameters));
+      69           1 :   nlopt_->set_ftol_rel(optimization_parameters_.f_rel);
+      70           1 :   nlopt_->set_ftol_abs(optimization_parameters_.f_abs);
+      71           1 :   nlopt_->set_xtol_rel(optimization_parameters_.x_rel);
+      72           1 :   nlopt_->set_xtol_abs(optimization_parameters_.x_abs);
+      73           1 :   nlopt_->set_maxeval(optimization_parameters_.max_iterations);
+      74           1 :   nlopt_->set_maxtime(optimization_parameters_.max_time);
+      75             : 
+      76           1 :   if (optimization_parameters_.random_seed < 0)
+      77           0 :     nlopt_srand_time();
+      78             :   else
+      79           1 :     nlopt_srand(optimization_parameters_.random_seed);
+      80             : 
+      81           1 :   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           1 : int PolynomialOptimizationNonLinear<_N>::optimize() {
+      91           1 :   optimization_info_ = OptimizationInfo();
+      92           1 :   int result         = nlopt::FAILURE;
+      93             : 
+      94           1 :   const std::chrono::high_resolution_clock::time_point t_start = std::chrono::high_resolution_clock::now();
+      95             : 
+      96           1 :   switch (optimization_parameters_.time_alloc_method) {
+      97           0 :     case NonlinearOptimizationParameters::kSquaredTime:
+      98             :     case NonlinearOptimizationParameters::kRichterTime:
+      99           0 :       result = optimizeTime();
+     100             :       break;
+     101           0 :     case NonlinearOptimizationParameters::kSquaredTimeAndConstraints:
+     102             :     case NonlinearOptimizationParameters::kRichterTimeAndConstraints:
+     103           0 :       result = optimizeTimeAndFreeConstraints();
+     104             :       break;
+     105           1 :     case NonlinearOptimizationParameters::kMellingerOuterLoop:
+     106           1 :       result = optimizeTimeMellingerOuterLoop();
+     107             :       break;
+     108             :     default:
+     109             :       break;
+     110             :   }
+     111             : 
+     112           1 :   const std::chrono::high_resolution_clock::time_point t_stop = std::chrono::high_resolution_clock::now();
+     113           1 :   optimization_info_.optimization_time                        = std::chrono::duration_cast<std::chrono::duration<double>>(t_stop - t_start).count();
+     114             : 
+     115           1 :   optimization_info_.stopping_reason = result;
+     116             : 
+     117           1 :   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             :     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             :     return nlopt::FAILURE;
+     154             :   }
+     155             : 
+     156             :   return result;
+     157             : }
+     158             : 
+     159             : template <int _N>
+     160           1 : int PolynomialOptimizationNonLinear<_N>::optimizeTimeMellingerOuterLoop() {
+     161           1 :   std::vector<double> segment_times;
+     162           1 :   poly_opt_.getSegmentTimes(&segment_times);
+     163             : 
+     164             :   // Save original segment times
+     165           2 :   std::vector<double> original_segment_times = segment_times;
+     166             : 
+     167           1 :   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           1 :     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           1 :     nlopt_->set_upper_bounds(std::numeric_limits<double>::max());
+     179           1 :     nlopt_->set_lower_bounds(kOptimizationTimeLowerBound);
+     180           1 :     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             :     return nlopt::FAILURE;
+     185             :   }
+     186             : 
+     187           1 :   double final_cost = std::numeric_limits<double>::max();
+     188           1 :   int    result     = nlopt::FAILURE;
+     189             : 
+     190             :   try {
+     191           1 :     result = nlopt_->optimize(segment_times, final_cost);
+     192             :   }
+     193           0 :   catch (std::exception& e) {
+     194           0 :     LOG(ERROR) << "error while running nlopt: " << e.what() << ". This likely means the optimization aborted early." << std::endl;
+     195           0 :     if (final_cost == std::numeric_limits<double>::max()) {
+     196             :       return nlopt::FAILURE;
+     197             :     }
+     198             : 
+     199           0 :     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           1 :   std::vector<double> relative_segment_times;
+     212           1 :   poly_opt_.getSegmentTimes(&relative_segment_times);
+     213           1 :   scaleSegmentTimesWithViolation();
+     214           1 :   std::vector<double> scaled_segment_times;
+     215           1 :   poly_opt_.getSegmentTimes(&scaled_segment_times);
+     216             : 
+     217             :   // Print all parameter after scaling
+     218           1 :   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           1 :   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           1 : double PolynomialOptimizationNonLinear<_N>::getCostAndGradientMellinger(std::vector<double>* gradients) {
+     258             :   // Weighting terms for different costs
+     259             :   // Retrieve the current segment times
+     260           1 :   std::vector<double> segment_times;
+     261           1 :   poly_opt_.getSegmentTimes(&segment_times);
+     262           1 :   const double J_d = poly_opt_.computeCost();
+     263             : 
+     264           1 :   if (poly_opt_.getNumberSegments() == 1) {
+     265           1 :     if (gradients != NULL) {
+     266           1 :       gradients->clear();
+     267           1 :       gradients->resize(poly_opt_.getNumberSegments(), 0.0);
+     268             :     }
+     269             : 
+     270           1 :     return J_d;
+     271             :   }
+     272             : 
+     273           0 :   if (gradients != NULL) {
+     274           0 :     const size_t n_segments = poly_opt_.getNumberSegments();
+     275             : 
+     276           0 :     gradients->clear();
+     277           0 :     gradients->resize(n_segments);
+     278             : 
+     279             :     // Initialize changed segment times for numerical derivative
+     280           0 :     std::vector<double> segment_times_bigger(n_segments);
+     281           0 :     const double        increment_time = 0.1;
+     282           0 :     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           0 :       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           0 :       double const_traj_time_corr = increment_time / (n_segments - 1.0);
+     289           0 :       for (size_t i = 0; i < segment_times_bigger.size(); ++i) {
+     290           0 :         if (i == n) {
+     291           0 :           segment_times_bigger[i] += increment_time;
+     292             :         } else {
+     293           0 :           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           0 :       for (double& t : segment_times_bigger) {
+     310           0 :         t = std::max(kOptimizationTimeLowerBound, t);
+     311             :       }
+     312             : 
+     313             :       // Update the segment times. This changes the polynomial coefficients.
+     314           0 :       poly_opt_.updateSegmentTimes(segment_times_bigger);
+     315           0 :       poly_opt_.solveLinear();
+     316             : 
+     317             :       // Calculate cost and gradient with new segment time
+     318           0 :       const double J_d_bigger = poly_opt_.computeCost();
+     319           0 :       const double dJd_dt     = (J_d_bigger - J_d) / increment_time;
+     320             : 
+     321             :       // Calculate the gradient
+     322           0 :       gradients->at(n) = dJd_dt;
+     323             :     }
+     324             : 
+     325             :     // Set again the original segment times from before calculating the
+     326             :     // numerical gradient
+     327           0 :     poly_opt_.updateSegmentTimes(segment_times);
+     328           0 :     poly_opt_.solveLinear();
+     329             :   }
+     330             : 
+     331             :   // Compute cost without gradient
+     332             :   return J_d;
+     333             : }
+     334             : 
+     335             : template <int _N>
+     336           1 : void PolynomialOptimizationNonLinear<_N>::scaleSegmentTimesWithViolation() {
+     337             : 
+     338             :   // Get trajectory
+     339           2 :   Trajectory traj;
+     340           1 :   poly_opt_.getTrajectory(&traj);
+     341             : 
+     342             :   // get constraints
+     343           1 :   double v_max_horizontal = 0.0;
+     344           1 :   double a_max_horizontal = 0.0;
+     345           1 :   double j_max_horizontal = 0.0;
+     346             : 
+     347           1 :   double v_max_vertical = 0.0;
+     348           1 :   double a_max_vertical = 0.0;
+     349           1 :   double j_max_vertical = 0.0;
+     350             : 
+     351           1 :   double v_max_heading = 0.0;
+     352           1 :   double a_max_heading = 0.0;
+     353           1 :   double j_max_heading = 0.0;
+     354             : 
+     355          13 :   for (const auto& constraint : inequality_constraints_) {
+     356          12 :     if (constraint->dimension <= 1) {
+     357           6 :       if (constraint->derivative == derivative_order::VELOCITY) {
+     358           2 :         v_max_horizontal = constraint->value;
+     359           4 :       } else if (constraint->derivative == derivative_order::ACCELERATION) {
+     360           2 :         a_max_horizontal = constraint->value;
+     361           2 :       } else if (constraint->derivative == derivative_order::JERK) {
+     362           2 :         j_max_horizontal = constraint->value;
+     363             :       }
+     364           6 :     } else if (constraint->dimension == 2) {
+     365           3 :       if (constraint->derivative == derivative_order::VELOCITY) {
+     366           1 :         v_max_vertical = constraint->value;
+     367           2 :       } else if (constraint->derivative == derivative_order::ACCELERATION) {
+     368           1 :         a_max_vertical = constraint->value;
+     369           1 :       } else if (constraint->derivative == derivative_order::JERK) {
+     370           1 :         j_max_vertical = constraint->value;
+     371             :       }
+     372           3 :     } else if (constraint->dimension == 3) {
+     373           3 :       if (constraint->derivative == derivative_order::VELOCITY) {
+     374           1 :         v_max_heading = constraint->value;
+     375           2 :       } else if (constraint->derivative == derivative_order::ACCELERATION) {
+     376           1 :         a_max_heading = constraint->value;
+     377           1 :       } else if (constraint->derivative == derivative_order::JERK) {
+     378           1 :         j_max_heading = constraint->value;
+     379             :       }
+     380             :     }
+     381             :   }
+     382             : 
+     383           1 :   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           1 :   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           1 :   std::vector<double> segment_times;
+     406           2 :   segment_times = traj.getSegmentTimes();
+     407           1 :   poly_opt_.updateSegmentTimes(segment_times);
+     408           1 :   poly_opt_.solveLinear();
+     409             : 
+     410           1 :   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           1 : }
+     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             :     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             :     return nlopt::FAILURE;
+     533             :   }
+     534             : 
+     535           0 :   return result;
+     536             : }
+     537             : 
+     538             : template <int _N>
+     539          12 : bool PolynomialOptimizationNonLinear<_N>::addMaximumMagnitudeConstraint(int dimension, int derivative, double maximum_value) {
+     540             : 
+     541          12 :   CHECK_GE(derivative, 0);
+     542          12 :   CHECK_GE(maximum_value, 0.0);
+     543             : 
+     544          24 :   std::shared_ptr<ConstraintData> constraint_data(new ConstraintData);
+     545          12 :   constraint_data->dimension   = dimension;
+     546          12 :   constraint_data->derivative  = derivative;
+     547          12 :   constraint_data->value       = maximum_value;
+     548          12 :   constraint_data->this_object = this;
+     549             : 
+     550             :   // Store the shared_ptrs such that their data will be destroyed later.
+     551          12 :   inequality_constraints_.push_back(constraint_data);
+     552             : 
+     553          12 :   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             :       return false;
+     561             :     }
+     562             :   }
+     563             : 
+     564             :   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           1 : double PolynomialOptimizationNonLinear<_N>::objectiveFunctionTimeMellingerOuterLoop(const std::vector<double>& segment_times, std::vector<double>& gradient,
+     618             :                                                                                     void* data) {
+     619           1 :   CHECK(!gradient.empty()) << "only with gradients possible, choose a gradient based method";
+     620           1 :   CHECK_NOTNULL(data);
+     621             : 
+     622           1 :   PolynomialOptimizationNonLinear<N>* optimization_data = static_cast<PolynomialOptimizationNonLinear<N>*>(data);  // wheee ...
+     623             : 
+     624           1 :   CHECK_EQ(segment_times.size(), optimization_data->poly_opt_.getNumberSegments());
+     625             : 
+     626           1 :   optimization_data->poly_opt_.updateSegmentTimes(segment_times);
+     627           1 :   optimization_data->poly_opt_.solveLinear();
+     628             :   double cost_trajectory;
+     629           1 :   if (!gradient.empty()) {
+     630           1 :     cost_trajectory = optimization_data->getCostAndGradientMellinger(&gradient);
+     631             :   } else {
+     632           0 :     cost_trajectory = optimization_data->getCostAndGradientMellinger(NULL);
+     633             :   }
+     634             : 
+     635           1 :   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           1 :   optimization_data->optimization_info_.n_iterations++;
+     646           1 :   optimization_data->optimization_info_.cost_trajectory = cost_trajectory;
+     647             : 
+     648           1 :   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             :   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..8343ad4c41e2270168d8b07402f3d31a991f3022 GIT binary patch literal 3492 zcmV;V4O{YwP)1 zRCt{2T@8ZkC*`d4X^3z@e&_HGVfyT7eR&8LFN`J-PPG=u+^w9G@(S zoLKdkddjj0z!8c;&2#wwV0ekIKm#kQddAj>_$5lVu$EatA^0P==YlF?O^CJ4_+Mx~ zscJ#q1XMpcTY#%xu>(h{ZcAGN2WVR<_5_iEpR~eSDpDhMc~ehW7AjJ-+04_?gV$*O zGXQ2Zb%ErvAa4J7p~O=kDu7Y78OUg7;F8{3q^8Qi1+_?lxn>GX>?4q%I50_6R6Q@e z>iq|&P-z@I?FtH>p|6XIoPZuJhX-0{2LO!HCP~IYtLWlxvBx-+^?-jY<@M*!^mx43 z8)bPuVwqkKTbYcw$A~b`o*bccs?Op~kAvh>p!yu5NQ5D-VqJ9`n~tg#ab8Ttd)2Dk zCxHi(4IfT;JV;W;^r{b_K|R~{-pkf4J1`)jc%Pr0BOfH7g`)3+0?=GgIhxkddgRXq zJ^m|!4BQ017)4{*DM~%?S$Ud?*hI52r_Ev!2k@awQ}AdP=CFspm$W-lb1^&8M)zNq z0?B7Q8pUr8al&X7jgf1Z7%uVnaudGNCkRy9+S-HQsudG_34?1ctQHnv`QENk&)rMq|)ykL>_751Q+o1OIbFpEZv-3;079 zr5`RyDQy5odj#`43{yAUWV6xNB-5;H9U>oS(G7~W-jx=)uP9Pu$}&BS^O+HY{Mr!L zSlTnQXakvgIuxPU_4uC}xo}DPs(lL&4DBzU;k}peF;lqJ9<633ug#tUkAI~J>B!}i zb2xzROMkY*T&AFhU;F<;JTLEiQ2)t#p z>E9lcD)&Cg#zC>O@B{W0bYZw)3^|*m>N7c$yfK|fgq>7vhX{um#Qq}0XFJoWg3?L!HGWS^nXw?nQ6ks`~Q|XV~8JjTX@-Xfq->_oe z$edOSn&apyM)yNuK;u|w-`+2$3OdkOb?P*%&tQ%*^%j)Ja@6Oc&!nE?Mf!Z7G|2NF z*s5@8(c|i{P+C;D@W;d2(8qt3FcZvMAqzSDtJ|6?5*J*u9&uroTHK|{R9daYuT4ym zIXioq4p@(gKe1xE)-z7U2lo7!`R59e}>w3wjja<=%&F!FOor zo(12WGg4!MwqzP90yn3DxJ41#IsX&`wXxnK8MDY!Uy6jzb%Lz(Ueyy%_?%}Y26QC) zlNms+S)iy_$Az)s>?OBMQf2f%wLHa?R3W#eKEpdqZO73bg@ukUynsEHGAebRrhwpJ zNg&{+u1PD8%YZtM7C}Im>_`{-un68v84Sy+5$J<=N%$h2<`j?ZwE$6%Ywa5j#H5Cr zI-k)1`(ATbN51nRkEZ|AY(fymWE1yn@6W`n_1>D6DF%Q4)n$rdmGT@7ur;#qb9vNL zZe=oDu%pe5Pw?1VbkcOZWBa4Yt0X3f%YO>m3d8?m)P?9`c=?*RXzB5DaGd*9&HM*YTjQ6OZ-!MY_WJwj~`ML z^w_8Pbv=@9AXh!C$4@ENdNfcp>CrwA->}DS+-$N4l7z{#ZCH_!RaIVUbo2Jo43=)Y zczoZ7pa$D4$-5~qn^(4UhQc^Zm_-z2pK)}AK{l)Y6)TN~;&})Ei zV(udzcjV+}RlV5Oe+4L%-*dxFb$}3|B|xs^X(juy=BsI1E1e3fOL$o=eU^nWgYRhvxGXH5FVD ze@u+FfzpyRDiFt4c@p9|@c5>-s3|F`s~;YU-!`S{(I)KKncLEwLIFy*x`}~kRua&{ zV;~3tdBd@4TD6pwJW%xwQP+;TJ-MRp4M%NZCqXz&nt@yTKGwkZ#uyM#()QUmq)u{^ z>9uY-Nop&PH_UNIn{cB|mxAbI*ySyg8%MYg;Ep{cRSZV414MCp+b(Eu9oOs$MQdAA zZ{}S7Kh>ClUtN7z&uQ9cq;zuq3NwE=*hy|pd{SVC;;E|RBskB8KNqH2#_d>m29mk}Dd3D# z1(;N5x4+n9&EvO8@KK@q;++?bcg(yAq;Z9F_Y61t%r~z>?azgiJ<>k(A6Rp-MTN4e zcvtBrnrF#Cs6z#gOI=Rt;e0Ou$0=sAN9h3@A(D(yXvq{CK)Wz!_peooH>H~ZkNS=& z-SW+gIMU+erdT77WKof+5S!H1Q?@9%KU~`BuC!jn_B@``6mpQhwx{`Fh=i8mmLYAT z22vRCc-7b)W)xf7$S3`y7I7)7)=DpyLWRCRmcnYQ)KgXc8%z0(rCeNoV<`ggHY^FXW13t{7(+YxP-kEN$g6p;nqY_Pt zkw?Rj6gpzK#jYKSKtw@cUN@K9cA1wrEd(N+s^vCx6%gAnZQrv=&ynbvyjdziHwdaH zG&d|$Qp5Bqe$@e(`u}-HdOZ*lHBt+o0uBhn14ClO#}i=wvgZ&=(QicS(nvPKBc6bO z-#U#q2Vo9x^dw9@Y;DiW0C0@Q{HuT=y=i5?{5n`}O6CSMra~>nuM!WxWqZjP%jvGR zbaCp+DUvgsnk8#D_EOQ3=9duuHc&EQ3V%oK&0AnSwo>}!TnUxv&bVm+aTw2C1Hrt*~fXVPk^~v zCp-=U4UT*&2)5@vSKowqHO@S88FAayQ`dAD2xek!9*d)m4PhefHa)h|>;SJ%$6Io` zD3;G^0XN&P`lD);s&T|1t~~ybk==y*Ow3$+2Yx5Tp56NU!p1hsc$7gz^sz^1RRi Sc_qRC0000 + + + + + + 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:12018564.9 %
Date:2023-12-06 07:59:45Functions:101376.9 %
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 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
<unnamed>58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
<unnamed>100.0 %7 / 7-0 / 0
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
<unnamed>100.0 %6 / 6-0 / 0
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
<unnamed>75.0 %6 / 8-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
<unnamed>100.0 %13 / 13100.0 %1 / 1
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
<unnamed>65.5 %19 / 29100.0 %1 / 1
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
<unnamed>91.8 %45 / 49100.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..f79e0f8f18 --- /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:12018564.9 %
Date:2023-12-06 07:59:45Functions:101376.9 %
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 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
<unnamed>58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
<unnamed>65.5 %19 / 29100.0 %1 / 1
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
<unnamed>75.0 %6 / 8-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
<unnamed>91.8 %45 / 49100.0 %6 / 6
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
<unnamed>100.0 %6 / 6-0 / 0
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
<unnamed>100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
<unnamed>100.0 %13 / 13100.0 %1 / 1
+
+
+ + + + +
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..31c1688d6d --- /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:12018564.9 %
Date:2023-12-06 07:59:45Functions:101376.9 %
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 %6 / 6-0 / 0
<unnamed>100.0 %6 / 6-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
<unnamed>91.8 %45 / 49100.0 %6 / 6
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
<unnamed>65.5 %19 / 29100.0 %1 / 1
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
<unnamed>100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
<unnamed>100.0 %13 / 13100.0 %1 / 1
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
<unnamed>58.5 %24 / 4166.7 %2 / 3
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
<unnamed>75.0 %6 / 8-0 / 0
+
+
+ + + + +
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..964c68771d --- /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:12018564.9 %
Date:2023-12-06 07:59:45Functions:101376.9 %
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 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.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..5d318b3aba --- /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:12018564.9 %
Date:2023-12-06 07:59:45Functions:101376.9 %
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 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
+
+
+ + + + +
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..f6abf85caa --- /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:12018564.9 %
Date:2023-12-06 07:59:45Functions:101376.9 %
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 %6 / 6-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
+
+
+ + + + +
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..6ca95fe20c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + 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:454991.8 %
Date:2023-12-06 07:59:45Functions:66100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Polynomial::Polynomial(int)1
eth_trajectory_generation::Polynomial::Polynomial(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)8
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double)30
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double, Eigen::Matrix<double, -1, 1, 0, -1, 1>*)30
eth_trajectory_generation::Polynomial::getCoefficients(int) const46
eth_trajectory_generation::Polynomial::evaluate(double, int) const942
+
+
+ + + +
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..63f8ddedab --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html @@ -0,0 +1,104 @@ + + + + + + + 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:454991.8 %
Date:2023-12-06 07:59:45Functions:66100.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)30
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double, Eigen::Matrix<double, -1, 1, 0, -1, 1>*)30
eth_trajectory_generation::Polynomial::Polynomial(int)1
eth_trajectory_generation::Polynomial::Polynomial(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)8
eth_trajectory_generation::Polynomial::getCoefficients(int) const46
eth_trajectory_generation::Polynomial::evaluate(double, int) const942
+
+
+ + + +
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..119c654237 --- /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:454991.8 %
Date:2023-12-06 07:59:45Functions: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_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          75 : 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           1 :   Polynomial(int N) : N_(N), coefficients_(N) {
+      54           1 :     coefficients_.setZero();
+      55           1 :   }
+      56             : 
+      57             :   // Assigns arbitrary coefficients to a polynomial.
+      58           8 :   Polynomial(int N, const Eigen::VectorXd& coeffs) : N_(N), coefficients_(coeffs) {
+      59           8 :     CHECK_EQ(N_, coeffs.size()) << "Number of coefficients has to match.";
+      60           8 :   }
+      61             : 
+      62          12 :   Polynomial(const Eigen::VectorXd& coeffs) : N_(coeffs.size()), coefficients_(coeffs) {
+      63           6 :   }
+      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          46 :   Eigen::VectorXd getCoefficients(int derivative = 0) const {
+     109          46 :     CHECK_LE(derivative, N_);
+     110          46 :     if (derivative == 0) {
+     111          56 :       return coefficients_;
+     112             :     } else {
+     113          72 :       Eigen::VectorXd result(N_);
+     114          36 :       result.setZero();
+     115          36 :       result.head(N_ - derivative) =
+     116          36 :           coefficients_.tail(N_ - derivative).cwiseProduct(base_coefficients_.block(derivative, derivative, 1, N_ - derivative).transpose());
+     117          36 :       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         942 :   double evaluate(double t, int derivative) const {
+     151         942 :     if (derivative >= N_) {
+     152             :       return 0.0;
+     153             :     }
+     154         942 :     double             result;
+     155         942 :     const int          tmp = N_ - 1;
+     156         942 :     Eigen::RowVectorXd row = base_coefficients_.block(derivative, 0, 1, N_);
+     157         942 :     result                 = row[tmp] * coefficients_[tmp];
+     158        7510 :     for (int j = tmp - 1; j >= derivative; --j) {
+     159        6568 :       result *= t;
+     160        6568 :       result += row[j] * coefficients_[j];
+     161             :     }
+     162         942 :     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          30 :   static void baseCoeffsWithTime(int N, int derivative, double t, Eigen::VectorXd* coeffs) {
+     209          30 :     CHECK_LT(derivative, N);
+     210          30 :     CHECK_GE(derivative, 0);
+     211             : 
+     212          30 :     coeffs->resize(N, 1);
+     213          30 :     coeffs->setZero();
+     214             :     // first coefficient doesn't get multiplied
+     215          30 :     (*coeffs)[derivative] = base_coefficients_(derivative, derivative);
+     216             : 
+     217          30 :     if (std::abs(t) < std::numeric_limits<double>::epsilon())
+     218             :       return;
+     219             : 
+     220          15 :     double t_power = t;
+     221             :     // now multiply increasing power of t towards the right
+     222         120 :     for (int j = derivative + 1; j < N; j++) {
+     223         105 :       (*coeffs)[j] = base_coefficients_(derivative, j) * t_power;
+     224         105 :       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          30 :   static Eigen::VectorXd baseCoeffsWithTime(int N, int derivative, double t) {
+     234          30 :     Eigen::VectorXd c(N);
+     235          30 :     baseCoeffsWithTime(N, derivative, t, &c);
+     236          30 :     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          18 :   static inline int getConvolutionLength(int data_size, int kernel_size) {
+     244          18 :     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..8f24c71ffff3f844576638ad70e091a0d44c0e7f GIT binary patch literal 1546 zcmV+l2KD)gP)`Q)pk1L5CSsgX9#cgqg5Z^1jf6iYx zFMzJ|UyLpw0XuLNAbu9!zz_+41P5>c=keG9yd*V5LP8gK)d<5sK-7cL5m>+o=O@gY zOZpOz%>oDT3P#i(khb%i0HT3tNQ+?z9#ArJN(6R4pgo;8`uPG1FwC{H zNnD_v6(yK}l?-E-1RsE&;F*n3Vh(Sv##YC%ALmMs@p=Ou`ve97YZveGxV}o(2&5Gg ze@j#mVRgI>-Zsqoi4?*H4iqKQ;1gpW$%27%OMWf{;o~H-wzX=~f;wbA471`g_`=as zMQn0Hg=lP%5-HRpbB_D#ao|e(>28Ux_Qs>O2SesHaI?pfH)f2BfZ}FR+$gxVD#GyY zA(>KAH;zMuA$fNM+W98F`;IUV%@-0s^?lU0FOWV_q$9B$h%%)eZ;G|vdc^&?u<2t- z-oy3rApU&4UO%tj7p))q|Gx42`&4UtKIP-rkHEj4_1d1l%{_PBBkfSUL0a}T$7dC$ zqAdl@&ZkV{MUG|52*S|NZXp>W_(ToLIS1=JO35lktIhGu%56PH{gjR@*LmbR9DqqP z7f^zIkWf}~U4zjP#uLSL3YYJ%)Ld)~W-~-Daf`evwFl|>Kx#9>Nb2ME3!HFNL=N#Y zjf2sm-VRO3G$5e+DQ>``2JF{^yuM(W&-YT+aX}^vC_Pv}v|@1ly2AKL6FyW@)$dX+ z33inJF#v()ysRA6g0tzsZP<=qHAyuE)rKFw; zvsTaiLtBCO0${M0coi!^owigg$EbGx(-=Xz)kqye75SK^KZ&{}OzA10>_;!rs2t;V zZwgQ~!c8u?$0T5j9VUSyYw!&OJX7p~D^aSsT-G~iKzu`h-J*+VpFEe%JZ5aS1Tp-LWSs@Xa2*R8loNNu0q1p85PgfUS%RRaeG?w0k_34bPJ}K5LSz9024P zMG4_>&dIu2+zMeF>5Hi0uL*Tk81AriH(BMax)0uTQ0(x+F~U(OC|tlA6~v7i`Uy)VtW5^Oz2q2l0tl z_6)y%M1fKs9kp|kGE1q;)*bKc7I3Gswa@7Pazs+XX^7_Z)lLh+MpIi?1&uLiygp^q zN*TM9{c#TSHa%Zk-GvfKyvB^2zop00z2FEbdi4y~IauU@@^Bt0(XkFyPC;@zrsVFo9}okp z-s>XV8uywlT&WiolB=}Rhg+k2VM!syLC-r2yNuCRg{tn;{Q`ZT wWIk4`eTJ_7U_; + + + + + + 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:192965.5 %
Date:2023-12-06 07:59:45Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimization<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const1
+
+
+ + + +
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..ac552af338 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html @@ -0,0 +1,84 @@ + + + + + + + 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:192965.5 %
Date:2023-12-06 07:59:45Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimization<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const1
+
+
+ + + +
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..01cc59ebc2 --- /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:192965.5 %
Date:2023-12-06 07:59:45Functions:11100.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_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           2 :   void getTrajectory(Trajectory* trajectory) const {
+     113           1 :     CHECK_NOTNULL(trajectory);
+     114           2 :     trajectory->setSegments(segments_);
+     115           1 :   }
+     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           1 :   void getSegments(Segment::Vector* segments) const {
+     178           1 :     CHECK_NOTNULL(segments);
+     179           1 :     *segments = segments_;
+     180           1 :   }
+     181             : 
+     182           4 :   void getSegmentTimes(std::vector<double>* segment_times) const {
+     183             :     CHECK(segment_times != nullptr);
+     184           4 :     *segment_times = segment_times_;
+     185           4 :   }
+     186             : 
+     187           0 :   void getFreeConstraints(std::vector<Eigen::VectorXd>* free_constraints) const {
+     188             :     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             :     return dimension_;
+     208             :   }
+     209           3 :   size_t getNumberSegments() const {
+     210             :     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             :     return n_free_constraints_;
+     220             :   }
+     221           0 :   int getDerivativeToOptimize() const {
+     222             :     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          50 : struct Constraint
+     290             : {
+     291          34 :   inline bool operator<(const Constraint& rhs) const {
+     292          34 :     if (vertex_idx < rhs.vertex_idx)
+     293             :       return true;
+     294          32 :     if (rhs.vertex_idx < vertex_idx)
+     295             :       return false;
+     296             : 
+     297          21 :     if (constraint_idx < rhs.constraint_idx)
+     298             :       return true;
+     299             :     if (rhs.constraint_idx < constraint_idx)
+     300             :       return false;
+     301             :     return false;
+     302             :   }
+     303             : 
+     304         100 :   inline bool operator==(const Constraint& rhs) const {
+     305         100 :     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..45a6c92eda95fd137b9c10f3ed72e56be7d7d1fa GIT binary patch literal 1756 zcmV<21|#{2P)16Bpgc=lV(U?bjRL8 zIK1=jkaQXiK1+B;bn(ODSrEQs-%|A4fG%N=bU@hBjtCo;1H!P5QI4Oa)bV_T+1K-S z!_+I{%YhhQWvU2vVM?>iyM`kmLLM;R8SFO!;pBQ!q1!%$iFhtSb1Ax8o{4U_KxU8C_;)Vg%DS<#*R{4N&uO- zZatQ@B@@cJzn>0u6a+vqrInOX1#3FXN;k+R!!bcsgFhtcqRbX<_E=B$Nao%;AXdk5 zWDaGWF(xph)f?Qjj4~B20SLLoI+9r8$0{xjo{G-mjXB40oLM=VK)*B|soRf?(b1RV zfl<_vdLEdY^npHSlKJrLVBIy$fuF0pn|qD+H@u73>&o- zMlO9D{@gQY74nJ-3iTuF4uAMVUW@6u?VO)T;CW#6e?&?k3m-P*4a2QY@h= zGYK>khOQlDl}giIwQDu(nbkit(c*?xPu+_CGEztq%1Vc%Ap4YwGbW=TjDzR55|@=H zii(zyO!X|8t;KcKA+amL+1hdH)EQ7)bTK19zGv`k9*yGGK7!E(~kd< zk8ZM7>xiD7L5d;r=na-NPAmWEoe zKUM^(PsSlImAxs#JHANWQbEue#c$j@_ovK-J>!SjnKS%%g|~Y?MnibHSxjjgaE}x} z){o^p+2>J;@3TqmX8ce!PzB%Iq(3 z)3SIiy2OL={#|nDxbAfvG`0;#ELyUWYZ@%VDqL=Mer&b0`on2dwaLZ(#}(gT8v_!p#7)`Q>hrp{yAw)S>DbUuA)Hy%k_$?pFMN$mZW=w!DZ zQIjE8+zt|Pz+#DnOQMf{gk*H|$0|`8(H{s^(Wb~`?ffy%m2jD}UdPr)Z>rnsD3b_* zG=vdFCpx|vM}7Va`H(r9V*A+gW2_sD|fLH7@?)|O({5df+H0000 + + + + + + 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:2023-12-06 07:59:45Functions: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_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..52df834b0e --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html @@ -0,0 +1,80 @@ + + + + + + + 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:2023-12-06 07:59:45Functions: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_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..4428df3066 --- /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:2023-12-06 07:59:45Functions:00-
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           4 : 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           1 :   void getTrajectory(Trajectory* trajectory) const {
+     182           1 :     poly_opt_.getTrajectory(trajectory);
+     183           1 :   }
+     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           1 :   PolynomialOptimization<N>& getPolynomialOptimizationRef() {
+     194             :     return poly_opt_;
+     195             :   }
+     196             : 
+     197           3 :   OptimizationInfo getOptimizationInfo() const {
+     198           5 :     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..1247ecf14bfaf217dbdf10efdb3e578385816e8b GIT binary patch literal 1665 zcmV-{27dX8P)DV0Y5T4P#sm^?!IqC55$ z!r?nt55j4DP7iw*V(KzS)H@GWMV%z;IxzM* z?pvKGyC`Wb$&P1R49WZnk=p|IH+8%(yP}TSYwD`jg#Q4J z|E`WlbEWoLbUBpOKq!_jTaO z?#Bpv!fpBGMfoFeVuY!6hnFGRXPKEj09>^}s^@+;ChQ-r2{8>9FN!9NyfzwQ z{LZgYHcG`2Y%I7_8xW_h)-r+;Yec;FVNqHw8LJviJm;L-PY+h+QKao>bs=ly=a-ue7_PR?WcE)zZ&Fp7^h&0E6b6|FIQFcgun z=2#IH7HdKWeS=U39Fw=1anRiFLXwPhNU^A&93gmowElr^$7R*pUgCWV`-M6RD91i( z2|c!jO{r%t@(wGV`t_ELHo;mOZFwuiEW*}+!D9;+; zFMqdF|3Zihws@ZBx`ZX7aNTJi)ck6x{~l+8O9~p5wB<;|bi;A5cO9{5%~mPQibzwk zDQg1J*v&Ilo~`K8MnNb(W!kPMbsE|D3nKLk#VWBWXRxdk94(b#^*URtcJ16S7$=gx zcQ`0;kzuPJiFy|WKp`>F^y!oR zabEWSs~Xs!&v$dWll^mZ2Y76*3QsG0QYBX1jm(8xfLFs=<$CpDZ3DyO{KEGxX7$SM zU8|!9U(uI;+-=#{ujRJ)`~|C@9rfC13_1=&u@LUYYe9ofPwDh(N z2oY}9p3NS2_9iYCu&#-n7T#B6K-d+%eC2p`to_B~4HIiwf8V2mX$v2J+Z*m5e6mkr zm$Y7roH^c5$`BgA;$9e%$=IZ7y+BST+d%E3;vd!rvQWv0^1<3_ZWJ*rT-j4eIRd;{ zW&Z=-9Dn&Mz`X`pSt7y>XVw~uuMFlT61_9{kk0_OuV@ryUczZD>@lAQg~!CDppc!Q zD2qLb=eU6ACEAYTGI(au!&$o{lSOmdp?6#S_}WZiFfq%pV6fc>Wwl%wVyeq$u8+i=qE*UmEDva|t@YyNs z)=aj=pzGKmRr22_?l8G->KJUc$BmAF=dR1CkC0)Ny5xX5_(tb1h7*mg;Zjag00000 LNkvXXu0mjfgnJw( 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..a0441ee4ed --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Segment::Segment(int, int)1
+
+
+ + + +
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..83d8a259de --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html @@ -0,0 +1,84 @@ + + + + + + + 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:2023-12-06 07:59:45Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Segment::Segment(int, int)1
+
+
+ + + +
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..b292785ff2 --- /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:2023-12-06 07:59:45Functions:11100.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           5 : class Segment {
+      45             : public:
+      46             :   typedef std::vector<Segment> Vector;
+      47             : 
+      48           1 :   Segment(int N, int D) : time_(0.0), N_(N), D_(D) {
+      49           1 :     polynomials_.resize(D_, Polynomial(N_));
+      50           1 :   }
+      51           4 :   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           9 :   int D() const {
+      59           9 :     return D_;
+      60             :   }
+      61           4 :   int N() const {
+      62           4 :     return N_;
+      63             :   }
+      64         250 :   double getTime() const {
+      65         250 :     return time_;
+      66             :   }
+      67             :   uint64_t getTimeNSec() const {
+      68             :     return static_cast<uint64_t>(kNumNSecPerSec * time_);
+      69             :   }
+      70             : 
+      71           9 :   void setTime(double time_sec) {
+      72           9 :     time_ = time_sec;
+      73             :   }
+      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..652bb2770fe633b7bfd66724304f0d13b32f0589 GIT binary patch literal 899 zcmV-}1AP36P)qCQt0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp=>6|DN>(5s*3JxVpIX0Jei9%qX_*`V=^Su=t}k8}$k+Lo zQV3*lfUAIX4^A*9mY*;H4sel;9pEFcF|iQ7psU6i`5pTPJRez68b7q{kF))I5fGxHM{M@-z2fHWg>*=Vf*&%dwcMp7Lj?MCA zapzN?R(W15E89(@&WJ>2y3td;=soD8&T$<3asKx8W-J%#4wQjaVUN{MjrF+3er(6K z?|gooSI1VVUI}?9CXc|zb;^|s=wxk~sGh{hanX|lI3JmRR87ecq+qD}ws7WE`?)zx1smZc9?WXLkQBrYmrXZ9nI0;r zvHOqcF`sF3)Tf8IkOKSL70K!vzp!c09lR47%_z?$7E-gVwNOj5vBY2E(ae(-RMUK7 zLaZgyB&+*+w|dhiTXqk!pGPwQxe-9ywj*x=R3a8!+Gbda*Y_c`KkcW<_6%gWE9 zdFKdoK=h3!SdWy0Fv48NgE=u%*j z6`(;B&6H-ES%Vp((*d|-%E#{0GDON0gU*?vt;NdmfsKuYofW`6qUzq*=!#2onPUy$ zUKP}@Mw~$r;i^VD{n#8Rp2kqY*cp%+jXiLsU|AB_s9w8JX+Bz2YtvIT$)VT!W<#$Y zI>Vh4*K{Y#y=q-I(x@ixG*8oB>m32<k 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..384b316633 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + 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:0320.0 %
Date:2023-12-06 07:59:45Functions:020.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>::LazyVariance() 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..3a04b4371c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html @@ -0,0 +1,88 @@ + + + + + + + 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:0320.0 %
Date:2023-12-06 07:59:45Functions:020.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>::LazyVariance() 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..b65154483c --- /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:0320.0 %
Date:2023-12-06 07:59:45Functions:020.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             :   }
+      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             :     return total_samples_;
+      65             :   }
+      66             : 
+      67           0 :   double Sum() const {
+      68             :     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             :     return max_;
+      81             :   }
+      82             : 
+      83           0 :   double Min() const {
+      84             :     return min_;
+      85             :   }
+      86             : 
+      87           0 :   double LazyVariance() const {
+      88           0 :     if (window_samples_ == 0) {
+      89             :       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             :   }
+     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..99b3a26b1866150aa650537725f0658ba5a7e50d GIT binary patch literal 1036 zcmV+n1oQieP)qV$0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vplDp8i`|TLK*x5@=RVUdZm7Oy-9hA#gU@oCH7@!S!|j!}|^L z`|zJg1u|H{J%I5uxWGuvpHKk~aGH)C;FQ%!%!J>>ca1Ufg)AK^A7Kq7ygw1$RMAs9 zHiQarhGX;|vbV!GfXM??iB5rGJVwj#kreEZ^peAb$wlHGl2?u*|Fm!5s_1>8R+1*;Uj6Yva-hoVnQ zz0C2b{1`lUJek*`dkSXYF;Iw=}s+cIa&uZi}Sf)92B zQ`L>liII_F7;#*GO0i_i-&se~>eqBU@5=J%n98+>e&^Ve2o8N&#`=(u^RNAoj(;K6 ze?doXtNom;THJL;kAR%HrZHY?nBmpv435v583FIEd0ci>?eP56%`_Nuvf<~MyXT$m zXU);y70%D7Sf0k%9G4fD<4!L~+~j)e1sXw5mu6c@^j#q_V3z|ugO7vP9Ho)VV(mUC zN_r_PyY609gfLn$(fX1;f3wh_--$0s!-<6n+O{ z)Y9k=vc<`ZCRurfGLcF!moNUYwRB?^#RFR@YHB8BDi>$@hrqMoBu8_8d}qo9kH4pk z0hcY499=v)B+EpJ)Ps`^ACANlA4-n)%XXIe3(wE5M6Kp0000 + + + + + + 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:244158.5 %
Date:2023-12-06 07:59:45Functions:2366.7 %
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::addSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)2
eth_trajectory_generation::Trajectory::setSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)2
+
+
+ + + +
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..25db598d35 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html @@ -0,0 +1,92 @@ + + + + + + + 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:244158.5 %
Date:2023-12-06 07:59:45Functions:2366.7 %
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&)2
eth_trajectory_generation::Trajectory::setSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)2
eth_trajectory_generation::Trajectory::clear()0
+
+
+ + + +
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..d66dd15ca7 --- /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:244158.5 %
Date:2023-12-06 07:59:45Functions:2366.7 %
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           0 : class Trajectory {
+      34             : public:
+      35           2 :   Trajectory() : D_(0), N_(0), max_time_(0.0) {
+      36             :   }
+      37           2 :   ~Trajectory() {
+      38           1 :   }
+      39             : 
+      40             :   bool        operator==(const Trajectory& rhs) const;
+      41             :   inline bool operator!=(const Trajectory& rhs) const {
+      42             :     return !operator==(rhs);
+      43             :   }
+      44             : 
+      45          41 :   int D() const {
+      46          41 :     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           2 :   void setSegments(const Segment::Vector& segments) {
+      66           2 :     CHECK(!segments.empty());
+      67             :     // Reset states.
+      68           2 :     D_        = segments.front().D();
+      69           2 :     N_        = segments.front().N();
+      70           2 :     max_time_ = 0.0;
+      71           2 :     segments_.clear();
+      72             : 
+      73           2 :     addSegments(segments);
+      74           2 :   }
+      75             : 
+      76           2 :   void addSegments(const Segment::Vector& segments) {
+      77           4 :     for (const Segment& segment : segments) {
+      78           2 :       CHECK_EQ(segment.D(), D_);
+      79           2 :       CHECK_EQ(segment.N(), N_);
+      80           2 :       max_time_ += segment.getTime();
+      81             :     }
+      82           2 :     segments_.insert(segments_.end(), segments.begin(), segments.end());
+      83           2 :   }
+      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           2 :   double getMinTime() const {
+      95           2 :     return 0.0;
+      96             :   }
+      97           2 :   double getMaxTime() const {
+      98           2 :     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..6f57a558d660c45ceb4654d3c05c54190f98ad5a GIT binary patch literal 979 zcmV;^11$WBP)`E3EiZ-#u$DfNr%EmSONpaUsyL& zv`fc^&;WWkChsA6`~Ctb9teeK2@K&eS%y_okVE8>!-V1@c2D3dMKRU{!;FHGjRH zuSoySF&Sx5o`UvZmb- zuKB0RN3ps1l7Js~_^Hb1O;7r$YOpRTlZq<^U|fB^PykNuka%?LaZ80+wb?PHhB z9&hiwu}>rJ>&-l(hqcisTDr2_b{Tz{i|8Ph&ceX0H8*s~efJGSJ~2x>eQ)6)t#{@K zS;@ZW&OwErV+LLy){z+1r^^k>R_S52`7&E56x1qUp@GxksNL9$U?Hz)vmMeJ(Q58N zmYSkV*-&`_M;zmO$PqQ`8f)ulABRDyWNFuMBxKJ$!l%eJsBSyJ;!&%`n3mRyG6v*S zERN#5Ld|huQc?v)2OCwS+{Q>%;B&e#E`dPC$UG5ccofSL19JfL9I*AVj);KxA~OVS zOob02@PN>{%w)u+28y|qLS7W?7b#xI)3p#}%BDD`QAQq`wX?A*VHM-z$)7Pg2{5R8 zXp6m&3^e(~>Hx0mXmM;9_C#C2w?r#W%-8|+L`JPn3A0t~o4xnVSy4wcHA1=V*>bkZ zdf=SlD+rLYRsNa4zL_~Zds3*Q_jiGq`?xhXE(iSbL0SX0xcwKjZ{-mZC>jZ z6dYluTox^fIFj{NYi<$(X33%J8wW)T%#5sX{{i6wS7^U7Wa$6^002ovPDHLkV1j0_ B)ky#V literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html new file mode 100644 index 0000000000..8fdf6f0f2a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + 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:2023-12-06 07:59:45Functions: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_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..9b96cd8ea8 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html @@ -0,0 +1,80 @@ + + + + + + + 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:2023-12-06 07:59:45Functions: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_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..8062bb0a9e --- /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:2023-12-06 07:59:45Functions:00-
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          17 : 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           4 :   Vertex(size_t dimension) : D_(dimension) {
+      52             :   }
+      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           2 :   typename Constraints::const_iterator cBegin() const {
+      93           2 :     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           9 :   typename Constraints::const_iterator cEnd() const {
+      99           9 :     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..ce4278780839271ff12ef6b741c85016395ff57e GIT binary patch literal 1058 zcmV+-1l{|IP)i(-d3HM7b1Z>6DG;+NEU+9*%$(2%muk?PEACw!= z{qdV~2qbWTYk+kxy}`&tKO+DRaFL81;F8wJM1*eY-ANgKfw(7UBY0p&*RsJZima~W9aq$YPgw$lDYjJBhyfSBw{70=#xWg(<$|NVq38Df zvG4o+cD3HX{c(vZK%a&q84M8SGqpG5^fxW;*^mv05LS#)Y)WSNTXtE0*$rQ+F>?oN{71)f3Qj|?<%SW~dkcKv9Ie6e^ z&bGG}f;B)P@rOujBS_M%l^AnLw$bj$UHOM`X>;d<1Y7CoA1(}&= zy7!p1j_D&JZGChJXq)CV=V>&R{A(4hL_=NX_%1AO46#w-%U@H}-35xt)(JD~&=nR) zl?2+QbqYxAI-TaGlxg)5O}Kf*$3pykVKO*o+5r0$yXMi6N2QZ{KAljEW=8` z>T6Blg(IaScV1I$X@*U%Y?O5+?)N5F6S8os!vbdvU}rZTfnf}o*e^PS*1aw3fie$5 z^>YF8REao8nkr+8qUqYo+SS&+GGJ;Mt#0&QPPvjh4exu>$tY*uJnQ-~d + + + + + + 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:41799442.0 %
Date:2023-12-06 07:59:45Functions: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 / 240.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 +
40.5%40.5%
+ 		40.5 %106 / 26235.3 %6 / 17
<unnamed>40.5 %106 / 26235.3 %6 / 17
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
<unnamed>40.9 %52 / 12746.2 %6 / 13
trajectory.cpp +
59.7%59.7%
+ 		59.7 %178 / 29847.8 %11 / 23
<unnamed>59.7 %178 / 29847.8 %11 / 23
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
<unnamed>50.5 %52 / 10354.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..74ab07f204 --- /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:41799442.0 %
Date:2023-12-06 07:59:45Functions: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 / 240.0 %0 / 4
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
vertex.cpp +
40.5%40.5%
+ 		40.5 %106 / 26235.3 %6 / 17
<unnamed>40.5 %106 / 26235.3 %6 / 17
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
<unnamed>40.9 %52 / 12746.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 +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
<unnamed>50.5 %52 / 10354.5 %6 / 11
trajectory.cpp +
59.7%59.7%
+ 		59.7 %178 / 29847.8 %11 / 23
<unnamed>59.7 %178 / 29847.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..9aef1e57ad --- /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:41799442.0 %
Date:2023-12-06 07:59:45Functions: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 / 240.0 %0 / 4
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
<unnamed>50.5 %52 / 10354.5 %6 / 11
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
<unnamed>40.9 %52 / 12746.2 %6 / 13
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
trajectory.cpp +
59.7%59.7%
+ 		59.7 %178 / 29847.8 %11 / 23
<unnamed>59.7 %178 / 29847.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 +
40.5%40.5%
+ 		40.5 %106 / 26235.3 %6 / 17
<unnamed>40.5 %106 / 26235.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..25799aa7a1 --- /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:41799442.0 %
Date:2023-12-06 07:59:45Functions: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 / 240.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 +
40.5%40.5%
+ 		40.5 %106 / 26235.3 %6 / 17
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
trajectory.cpp +
59.7%59.7%
+ 		59.7 %178 / 29847.8 %11 / 23
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.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..657b180a58 --- /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:41799442.0 %
Date:2023-12-06 07:59:45Functions: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 / 240.0 %0 / 4
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
vertex.cpp +
40.5%40.5%
+ 		40.5 %106 / 26235.3 %6 / 17
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
trajectory.cpp +
59.7%59.7%
+ 		59.7 %178 / 29847.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..fa1dfd8700 --- /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:41799442.0 %
Date:2023-12-06 07:59:45Functions: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 / 240.0 %0 / 4
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
trajectory.cpp +
59.7%59.7%
+ 		59.7 %178 / 29847.8 %11 / 23
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
vertex.cpp +
40.5%40.5%
+ 		40.5 %106 / 26235.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..f0868077f9 --- /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:0240.0 %
Date:2023-12-06 07:59:45Functions: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..d7f157600c --- /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:0240.0 %
Date:2023-12-06 07:59:45Functions: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..13a201ae04 --- /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:0240.0 %
Date:2023-12-06 07:59:45Functions: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           0 :     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           0 :   using namespace derivative_order;
+      43           0 :   if (string == "position") {
+      44             :     return POSITION;
+      45           0 :   } else if (string == "velocity") {
+      46             :     return VELOCITY;
+      47           0 :   } else if (string == "acceleration") {
+      48             :     return ACCELERATION;
+      49           0 :   } else if (string == "jerk") {
+      50             :     return JERK;
+      51           0 :   } else if (string == "snap") {
+      52             :     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           0 :     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           0 :   using namespace derivative_order;
+      77           0 :   if (string == "orientation") {
+      78             :     return ORIENTATION;
+      79           0 :   } else if (string == "angular_velocity") {
+      80             :     return ANGULAR_VELOCITY;
+      81           0 :   } else if (string == "angular_acceleration") {
+      82             :     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..adc83232603d9fb7f4a5b693f8d27b6804e6e6b4 GIT binary patch literal 468 zcmV;_0W1EAP)KSM3$UAPCeAU?;l)-G7zF2!2AMt^Lzik~THoI|NReICPWdJs#$dBc2ckz~l3S zr~(UwF7gMk8E zVNBTr&i47kL(Tv;u5)1+4=FO}M8OHkmr^FkDU$S{T)B!^6r#IU;$@&Y^RAV}OeB4p z5oUiVh$3$PPZ@hbRORbMk>)bq$SbRdRYG?5s}kN+<`e~;z7@G{VKg`!nXNFI z8qSQ@EBp$hi+^o1qR3I-?44iUTswMhy`!o=I_S;nxFSzd8tKq+D@XM2xZ4>0W=#wo zSCUFyyL_|b&UA{qovrmpmA4bcDx<Z$;n{wRtEpiC0000< KMNUMnLSTZlq03qT literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html new file mode 100644 index 0000000000..47c8b4815e --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.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:5210350.5 %
Date:2023-12-06 07:59:45Functions: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::Polynomial::scalePolynomialInTime(double)4
eth_trajectory_generation::Polynomial::convolve(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)12
eth_trajectory_generation::Polynomial::selectMinMaxCandidatesFromRoots(double, double, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::vector<double, std::allocator<double> >*)18
eth_trajectory_generation::Polynomial::computeMinMaxCandidates(double, double, int, std::vector<double, std::allocator<double> >*) const18
eth_trajectory_generation::Polynomial::getRoots(int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*) const18
eth_trajectory_generation::computeBaseCoefficients(int)42
+
+
+ + + +
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..48c4ebd623 --- /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:5210350.5 %
Date:2023-12-06 07:59:45Functions: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)4
eth_trajectory_generation::Polynomial::selectMinMaxCandidatesFromRoots(double, double, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::vector<double, std::allocator<double> >*)18
eth_trajectory_generation::Polynomial::convolve(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)12
eth_trajectory_generation::computeBaseCoefficients(int)42
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> >*) const18
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>*) const18
+
+
+ + + +
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..80595c660a --- /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:5210350.5 %
Date:2023-12-06 07:59:45Functions: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          18 : bool Polynomial::getRoots(int derivative, Eigen::VectorXcd* roots) const {
+      31          18 :   return findRootsJenkinsTraub(getCoefficients(derivative), roots);
+      32             : }
+      33             : 
+      34             : /* selectMinMaxCandidatesFromRoots() //{ */
+      35             : 
+      36          18 : bool Polynomial::selectMinMaxCandidatesFromRoots(double t_start, double t_end, const Eigen::VectorXcd& roots_derivative_of_derivative,
+      37             :                                                  std::vector<double>* candidates) {
+      38          18 :   CHECK_NOTNULL(candidates);
+      39          18 :   if (t_start > t_end) {
+      40           0 :     LOG(WARNING) << "t_start is greater than t_end.";
+      41           0 :     return false;
+      42             :   }
+      43          18 :   candidates->clear();
+      44          18 :   candidates->reserve(roots_derivative_of_derivative.size() + 2);
+      45             :   // Put start and end in, as they are valid candidates.
+      46          18 :   candidates->push_back(t_start);
+      47          18 :   candidates->push_back(t_end);
+      48         168 :   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         150 :     if (std::abs(roots_derivative_of_derivative[i].imag()) > std::numeric_limits<double>::epsilon()) {
+      51          82 :       continue;
+      52             :     }
+      53          98 :     const double candidate = roots_derivative_of_derivative[i].real();
+      54             : 
+      55             :     // Do not evaluate points outside the domain.
+      56          98 :     if (candidate < t_start || candidate > t_end) {
+      57          30 :       continue;
+      58             :     } else {
+      59          68 :       candidates->push_back(candidate);
+      60             :     }
+      61             :   }
+      62             :   return true;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* computeMinMaxCandidates() //{ */
+      68             : 
+      69          18 : bool Polynomial::computeMinMaxCandidates(double t_start, double t_end, int derivative, std::vector<double>* candidates) const {
+      70          18 :   CHECK_NOTNULL(candidates);
+      71          18 :   candidates->clear();
+      72          18 :   if (N_ - derivative - 1 < 0) {
+      73           0 :     LOG(WARNING) << "N - derivative - 1 has to be at least 0.";
+      74           0 :     return false;
+      75             :   }
+      76          36 :   Eigen::VectorXcd roots;
+      77          18 :   bool             success = getRoots(derivative + 1, &roots);
+      78          18 :   if (!success) {
+      79           0 :     VLOG(1) << "Couldn't find roots, polynomial may be constant.";
+      80             :   }
+      81          18 :   if (!selectMinMaxCandidatesFromRoots(t_start, t_end, roots, candidates)) {
+      82           0 :     return false;
+      83             :   }
+      84             :   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             :     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             :     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             :   return true;
+     149             : }
+     150             : 
+     151             : //}
+     152             : 
+     153             : /* computeBaseCoefficients() //{ */
+     154             : 
+     155          42 : Eigen::MatrixXd computeBaseCoefficients(int N) {
+     156          42 :   Eigen::MatrixXd base_coefficients(N, N);
+     157             : 
+     158          42 :   base_coefficients.setZero();
+     159          42 :   base_coefficients.row(0).setOnes();
+     160             : 
+     161          42 :   const int DEG   = N - 1;
+     162          42 :   int       order = DEG;
+     163         924 :   for (int n = 1; n < N; n++) {
+     164       11466 :     for (int i = DEG - order; i < N; i++) {
+     165       10584 :       base_coefficients(n, i) = (order - DEG + i) * base_coefficients(n - 1, i);
+     166             :     }
+     167         882 :     order--;
+     168             :   }
+     169          42 :   return base_coefficients;
+     170             : }
+     171             : 
+     172             : //}
+     173             : 
+     174             : /* convolve() //{ */
+     175             : 
+     176          12 : Eigen::VectorXd Polynomial::convolve(const Eigen::VectorXd& data, const Eigen::VectorXd& kernel) {
+     177          12 :   const int       convolution_dimension = getConvolutionLength(data.size(), kernel.size());
+     178          12 :   Eigen::VectorXd convolved             = Eigen::VectorXd::Zero(convolution_dimension);
+     179          24 :   Eigen::VectorXd kernel_reverse        = kernel.reverse();
+     180             : 
+     181         180 :   for (int i = 0; i < convolution_dimension; i++) {
+     182         168 :     const int data_idx = i - kernel.size() + 1;
+     183             : 
+     184         168 :     int lower_bound = std::max(0, -data_idx);
+     185         168 :     int upper_bound = std::min(kernel.size(), data.size() - data_idx);
+     186             : 
+     187         848 :     for (int kernel_idx = lower_bound; kernel_idx < upper_bound; ++kernel_idx) {
+     188         680 :       convolved[i] += kernel_reverse[kernel_idx] * data[data_idx + kernel_idx];
+     189             :     }
+     190             :   }
+     191          12 :   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           4 : void Polynomial::scalePolynomialInTime(double scaling_factor) {
+     219           4 :   double scale = 1.0;
+     220          44 :   for (int n = 0; n < N_; n++) {
+     221          40 :     coefficients_[n] *= scale;
+     222          40 :     scale *= scaling_factor;
+     223             :   }
+     224           4 : }
+     225             : 
+     226             : //}
+     227             : 
+     228             : /* offsetPolynomial() //{ */
+     229             : 
+     230           0 : void Polynomial::offsetPolynomial(const double offset) {
+     231           0 :   if (coefficients_.size() == 0)
+     232             :     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..a87c96c5061be37e1ef3e5e2269271e2ce3928cc GIT binary patch literal 1168 zcmV;B1aJF^P)0{{R3u754n0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpo(kL0Z@CqV@W~ zKLIBK&)wg)Re=bu;4+}Uwq9VdA^%NOfE`>-#s=__)?z~@c;e3rW#}Vt^_AI3aNs83 z3dvSo`jLzUi3;!uM{hlW)^-Q02o~7b(!nu^7hW@w1qB);d_^}29uy)tO^_EUlAZa!Xm5I9fyEo4zU!W>Pv_p|FO!l7 z+WhB|7ITLnpd8UeVo)^c(osjCa*^aedZ*KTiGpJ*nsQ(>>Mo#*&Py7lnqGG<1R?cK zays9todq{?lq4~mLSsFg|3bg=uGrS zTHhoA=XmSyaXXGzQg4@g-2Bo!@c}~|*O^5Ws8L8+5ru;%^>F|tsia^;LUK2ONg=gR zKF{Mgu2Kr8P&Y$Se6x_Mem(%uA1R*KN@-?^B_v_jIHa{x(JARQgJ1Bs^8;m?zE+`W zP_*<^JN~~}I3o!c^le7IZ#dESfLJQF4R_k)mc^uex-J|C`X~$y4IUj7l^Br;$D_;D zacro~E{wjtxnkUbhzj*9(3c{SLx#Hp9*Ko(Mwr5p-PCYll_RCGwV2Eepa~go)2z>}20)uiSm)-c^H*1QEQ(My47b8`eXA-2UjcMcC57}Tp4BT4-)uW{~d?bvIeIP95cRp@7 z!iL5*v%x&tnbBY`#UpVCr3L3ZAIk3-HkD04pIt6sX+z_> zujDwqw4~P)>0632rV3zc1N`ZurB%waJi*rmc$m3cv2XptMrFvt8^&Lhg)?9Jv^slg z(`W+n?KL#Xt%fIdDMQ^%Y_D(I8R%_Yvu^vVv(Dazal?k;^$dF{Ph}h!0otu#RmKw) z>h$3Ryv_(%k)sZ*sP)9)1DXKv>)OI8J2dspeX4A+ + + + + + + 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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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 +
88.9%88.9%
+ 		88.9 %346 / 389100.0 %12 / 12
<unnamed>88.9 %346 / 389100.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..839de9e8cc --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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 +
88.9%88.9%
+ 		88.9 %346 / 389100.0 %12 / 12
<unnamed>88.9 %346 / 389100.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..1b7d25d31d --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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 +
88.9%88.9%
+ 		88.9 %346 / 389100.0 %12 / 12
<unnamed>88.9 %346 / 389100.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..8553f99999 --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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 +
88.9%88.9%
+ 		88.9 %346 / 389100.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..d8320e5f09 --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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 +
88.9%88.9%
+ 		88.9 %346 / 389100.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..9e4794988e --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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 +
88.9%88.9%
+ 		88.9 %346 / 389100.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..052df425ee --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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*)18
eth_trajectory_generation::rpolyWrapper(double*, int*, double*, double*)18
eth_trajectory_generation::findLastNonZeroCoeff(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)18
eth_trajectory_generation::findRootsJenkinsTraub(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*)18
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*)74
eth_trajectory_generation::rpoly_impl::RealIT_ak1(int*, int*, double*, int, double*, int, double*, double*, double*, double*, double*)83
eth_trajectory_generation::rpoly_impl::Fxshfr_ak1(int, int*, double, double, double*, int, double*, int, double*, double*, double*, double*, double*)84
eth_trajectory_generation::rpoly_impl::Quad_ak1(double, double, double, double*, double*, double*, double*)164
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*)342
eth_trajectory_generation::rpoly_impl::nextK_ak1(int, int, double, double, double, double*, double*, double*, double*, double*)342
eth_trajectory_generation::rpoly_impl::calcSC_ak1(int, double, double, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double, double, double*)546
eth_trajectory_generation::rpoly_impl::QuadSD_ak1(int, double, double, double*, double*, double*, double*)776
+
+
+ + + +
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..08e5b74bef --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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*)84
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*)74
eth_trajectory_generation::rpoly_impl::QuadSD_ak1(int, double, double, double*, double*, double*, double*)776
eth_trajectory_generation::rpoly_impl::RealIT_ak1(int*, int*, double*, int, double*, int, double*, double*, double*, double*, double*)83
eth_trajectory_generation::rpoly_impl::calcSC_ak1(int, double, double, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double, double, double*)546
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*)342
eth_trajectory_generation::rpoly_impl::Quad_ak1(double, double, double, double*, double*, double*, double*)164
eth_trajectory_generation::rpoly_impl::nextK_ak1(int, int, double, double, double, double*, double*, double*, double*, double*)342
eth_trajectory_generation::rpoly_impl::rpoly_ak1(double*, int*, double*, double*)18
eth_trajectory_generation::rpolyWrapper(double*, int*, double*, double*)18
eth_trajectory_generation::findLastNonZeroCoeff(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)18
eth_trajectory_generation::findRootsJenkinsTraub(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*)18
+
+
+ + + +
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..48c0e7f7a0 --- /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:34638988.9 %
Date:2023-12-06 07:59:45Functions: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          18 : int findLastNonZeroCoeff(const Eigen::VectorXd& coefficients) {
+      60          18 :   int last_non_zero_coefficient = -1;
+      61             : 
+      62             :   // Find last non-zero coefficient:
+      63          54 :   for (int i = coefficients.size() - 1; i != -1; i--) {
+      64          54 :     if (std::abs(coefficients(i)) >= std::numeric_limits<double>::min()) {
+      65             :       last_non_zero_coefficient = i;
+      66             :       break;
+      67             :     }
+      68             :   }
+      69          18 :   return last_non_zero_coefficient;
+      70             : }
+      71             : 
+      72             : //}
+      73             : 
+      74             : /* findRootsJenkinsTraub() //{ */
+      75             : 
+      76          18 : bool findRootsJenkinsTraub(const Eigen::VectorXd& coefficients_increasing, Eigen::VectorXcd* roots) {
+      77             :   // Remove trailing zeros.
+      78          18 :   const int last_non_zero_coefficient = findLastNonZeroCoeff(coefficients_increasing);
+      79          18 :   if (last_non_zero_coefficient == -1) {
+      80             :     // The polynomial has all zero coefficients and has no roots.
+      81           0 :     roots->resize(0);
+      82           0 :     return true;
+      83             :   }
+      84             : 
+      85             :   // Reverse coefficients in descending order.
+      86          36 :   Eigen::VectorXd coefficients_decreasing = coefficients_increasing.head(last_non_zero_coefficient + 1).reverse();
+      87             : 
+      88          18 :   const int n_coefficients = coefficients_decreasing.size();
+      89          18 :   if (n_coefficients < 2) {
+      90             :     // The polynomial is 0th order and has no roots.
+      91          18 :     roots->resize(0);
+      92             :     return true;
+      93             :   }
+      94          18 :   int     degree     = n_coefficients - 1;
+      95          18 :   double* polynomial = new double[kRpolyMaxDegree + 1];
+      96          18 :   double* roots_real = new double[kRpolyMaxDegree];
+      97          18 :   double* roots_imag = new double[kRpolyMaxDegree];
+      98             : 
+      99         186 :   for (size_t i = 0; i < n_coefficients; i++) {
+     100         168 :     polynomial[i] = coefficients_decreasing(i);
+     101             :   }
+     102             : 
+     103          18 :   rpolyWrapper(polynomial, &degree, roots_real, roots_imag);
+     104          18 :   if (degree > 0) {
+     105          18 :     roots->resize(degree);
+     106         168 :     for (int i = 0; i < degree; ++i) {
+     107         150 :       (*roots)[i] = std::complex<double>(roots_real[i], roots_imag[i]);
+     108             :     }
+     109             :   }
+     110             : 
+     111          18 :   delete[] polynomial;
+     112          18 :   delete[] roots_real;
+     113          18 :   delete[] roots_imag;
+     114             : 
+     115          18 :   if (degree > 0) {
+     116             :     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          18 : void rpoly_ak1(double op[MDP1], int* Degree, double zeror[MAXDEGREE], double zeroi[MAXDEGREE]) {
+     149          18 :   int i, j, jj, l, N, NM1, NN, NZ, zerok;
+     150             : 
+     151          18 :   double K[MDP1], p[MDP1], pt[MDP1], qp[MDP1], temp[MDP1];
+     152          18 :   double bnd, df, dx, factor, ff, moduli_max, moduli_min, sc, x, xm;
+     153          18 :   double aa, bb, cc, lzi, lzr, sr, szi, szr, t, xx, xxx, yy;
+     154             : 
+     155          18 :   const double RADFAC = 3.14159265358979323846 / 180;  // Degrees-to-radians conversion factor = pi/180
+     156          18 :   const double lb2    = log(2.0);                      // Dummy variable to avoid re-calculating this value in loop below
+     157          18 :   const double lo     = FLT_MIN / DBL_EPSILON;
+     158          18 :   const double cosr   = cos(94.0 * RADFAC);  // = -0.069756474
+     159          18 :   const double sinr   = sin(94.0 * RADFAC);  // = 0.99756405
+     160             : 
+     161          18 :   if ((*Degree) > MAXDEGREE) {
+     162           0 :     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          18 :   if (op[0] != 0) {
+     170             :     N  = *Degree;
+     171          24 :     xx = sqrt(0.5);  // = 0.70710678
+     172          24 :     yy = -xx;
+     173             : 
+     174             :     // Remove zeros at the origin, if any
+     175             :     j = 0;
+     176          24 :     while (op[N] == 0) {
+     177           6 :       zeror[j] = zeroi[j] = 0.0;
+     178           6 :       N--;
+     179           6 :       j++;
+     180             :     }  // End while (op[N] == 0)
+     181             : 
+     182          18 :     NN = N + 1;
+     183             : 
+     184             :     // Make a copy of the coefficients
+     185         180 :     for (i = 0; i < NN; i++)
+     186         162 :       p[i] = op[i];
+     187             : 
+     188         100 :     while (N >= 1) {  // Main loop
+     189             :       // Start the algorithm for one zero
+     190         100 :       if (N <= 2) {
+     191             :         // Calculate the final zero or pair of zeros
+     192          18 :         if (N < 2) {
+     193           0 :           zeror[(*Degree) - 1] = -(p[1] / p[0]);
+     194           0 :           zeroi[(*Degree) - 1] = 0.0;
+     195             :         }       // End if (N < 2)
+     196             :         else {  // else N == 2
+     197          18 :           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             :         break;
+     200             :       }  // End if (N <= 2)
+     201             : 
+     202             :       // Find the largest and smallest moduli of the coefficients
+     203             : 
+     204             :       moduli_max = 0.0;
+     205             :       moduli_min = FLT_MAX;
+     206             : 
+     207         718 :       for (i = 0; i < NN; i++) {
+     208         636 :         x = fabs(p[i]);
+     209         636 :         if (x > moduli_max)
+     210         525 :           moduli_max = x;
+     211         636 :         if ((x != 0) && (x < moduli_min))
+     212         111 :           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          82 :       sc = lo / moduli_min;
+     224             : 
+     225          82 :       if (((sc <= 1.0) && (moduli_max >= 10)) || ((sc > 1.0) && (FLT_MAX / sc >= moduli_max))) {
+     226          14 :         sc     = ((sc == 0) ? FLT_MIN : sc);
+     227          14 :         l      = (int)(log(sc) / lb2 + 0.5);
+     228          14 :         factor = pow(2.0, l);
+     229          14 :         if (factor != 1.0) {
+     230         177 :           for (i = 0; i < NN; i++)
+     231         163 :             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         718 :       for (i = 0; i < NN; i++)
+     239         636 :         pt[i] = fabs(p[i]);
+     240          82 :       pt[N] = -(pt[N]);
+     241             : 
+     242          82 :       NM1 = N - 1;
+     243             : 
+     244             :       // Compute upper estimate of bound
+     245             : 
+     246          82 :       x = exp((log(-pt[N]) - log(pt[0])) / (double)N);
+     247             : 
+     248          82 :       if (pt[NM1] != 0) {
+     249             :         // If Newton step at the origin is better, use it
+     250          82 :         xm = -pt[N] / pt[NM1];
+     251          82 :         x  = ((xm < x) ? xm : x);
+     252             :       }  // End if (pt[NM1] != 0)
+     253             : 
+     254             :       // Chop the interval (0, x) until ff <= 0
+     255             : 
+     256             :       xm = x;
+     257         116 :       do {
+     258         116 :         x  = xm;
+     259         116 :         xm = 0.1 * x;
+     260         116 :         ff = pt[0];
+     261        1112 :         for (i = 1; i < NN; i++)
+     262         996 :           ff = ff * xm + pt[i];
+     263         116 :       } while (ff > 0);  // End do-while loop
+     264             : 
+     265             :       dx = x;
+     266             : 
+     267             :       // Do Newton iteration until x converges to two decimal places
+     268             : 
+     269         326 :       while (fabs(dx / x) > 0.005) {
+     270             :         df = ff = pt[0];
+     271        1666 :         for (i = 1; i < N; i++) {
+     272        1422 :           ff = x * ff + pt[i];
+     273        1422 :           df = x * df + ff;
+     274             :         }  // End for i
+     275         244 :         ff = x * ff + pt[N];
+     276         244 :         dx = ff / df;
+     277         244 :         x -= dx;
+     278             :       }  // End while loop
+     279             : 
+     280         554 :       bnd = x;
+     281             : 
+     282             :       // Compute the derivative as the initial K polynomial and do 5 steps with
+     283             :       // no shift
+     284             : 
+     285         554 :       for (i = 1; i < N; i++)
+     286         472 :         K[i] = (double)(N - i) * p[i] / ((double)N);
+     287          82 :       K[0] = p[0];
+     288             : 
+     289          82 :       aa    = p[N];
+     290          82 :       bb    = p[NM1];
+     291          82 :       zerok = ((K[NM1] == 0) ? 1 : 0);
+     292             : 
+     293         492 :       for (jj = 0; jj < 5; jj++) {
+     294         410 :         cc = K[NM1];
+     295         410 :         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         410 :           t = -aa / cc;
+     308        2770 :           for (i = 0; i < NM1; i++) {
+     309        2360 :             j    = NM1 - i;
+     310        2360 :             K[j] = t * K[j - 1] + p[j];
+     311             :           }  // End for i
+     312         410 :           K[0]  = p[0];
+     313         410 :           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         636 :       for (i = 0; i < N; i++)
+     320         554 :         temp[i] = K[i];
+     321             : 
+     322             :       // Loop to select the quadratic corresponding to each new shift
+     323             : 
+     324          84 :       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          84 :         xxx = -(sinr * yy) + cosr * xx;
+     330          84 :         yy  = sinr * xx + cosr * yy;
+     331          84 :         xx  = xxx;
+     332          84 :         sr  = bnd * xx;
+     333             : 
+     334             :         // Second stage calculation, fixed quadratic
+     335             : 
+     336          84 :         Fxshfr_ak1(20 * jj, &NZ, sr, bnd, K, N, p, NN, qp, &lzi, &lzr, &szi, &szr);
+     337             : 
+     338          84 :         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          82 :           j        = (*Degree) - N;
+     346          82 :           zeror[j] = szr;
+     347          82 :           zeroi[j] = szi;
+     348          82 :           NN       = NN - NZ;
+     349          82 :           N        = NN - 1;
+     350         610 :           for (i = 0; i < NN; i++)
+     351         528 :             p[i] = qp[i];
+     352          82 :           if (NZ != 1) {
+     353          26 :             zeror[j + 1] = lzr;
+     354          26 :             zeroi[j + 1] = lzi;
+     355             :           }  // End if (NZ != 1)
+     356             :           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          28 :           for (i = 0; i < N; i++)
+     363          26 :             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          82 :       if (jj > 20) {
+     371           0 :         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           0 :     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             :   return;
+     387             : }  // End rpoly_ak1
+     388             : 
+     389          84 : 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          84 :   int    fflag, i, iFlag, j, spass, stry, tFlag, vpass, vtry;
+     401          84 :   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          84 :   double qk[MDP1], svk[MDP1];
+     403             : 
+     404          84 :   *NZ   = 0;
+     405          84 :   betav = betas = 0.25;
+     406          84 :   u             = -(2.0 * sr);
+     407          84 :   oss           = sr;
+     408          84 :   ovv = v = bnd;
+     409             : 
+     410             :   // Evaluate polynomial by synthetic division
+     411          84 :   QuadSD_ak1(NN, u, v, p, qp, &a, &b);
+     412             : 
+     413          84 :   tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
+     414             : 
+     415         272 :   for (j = 0; j < L2; j++) {
+     416             :     // Calculate next K polynomial and estimate v
+     417         270 :     nextK_ak1(N, tFlag, a, b, a1, &a3, &a7, K, qk, qp);
+     418         270 :     tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
+     419         270 :     newest_ak1(tFlag, &ui, &vi, a, a1, a3, a7, b, c, d, f, g, h, u, v, K, N, p);
+     420             : 
+     421         270 :     vv = vi;
+     422             : 
+     423             :     // Estimate s
+     424             : 
+     425         270 :     ss = ((K[N - 1] != 0.0) ? -(p[N] / K[N - 1]) : 0.0);
+     426             : 
+     427         270 :     ts = tv = 1.0;
+     428             : 
+     429         270 :     if ((j != 0) && (tFlag != 3)) {
+     430             :       // Compute relative measures of convergence of s and v sequences
+     431             : 
+     432         186 :       tv = ((vv != 0.0) ? fabs((vv - ovv) / vv) : tv);
+     433         186 :       ts = ((ss != 0.0) ? fabs((ss - oss) / ss) : ts);
+     434             : 
+     435             :       // If decreasing, multiply the two most recent convergence measures
+     436             : 
+     437         186 :       tvv = ((tv < otv) ? tv * otv : 1.0);
+     438         186 :       tss = ((ts < ots) ? ts * ots : 1.0);
+     439             : 
+     440             :       // Compare with convergence criteria
+     441             : 
+     442         186 :       vpass = ((tvv < betav) ? 1 : 0);
+     443         186 :       spass = ((tss < betas) ? 1 : 0);
+     444             : 
+     445         186 :       if ((spass) || (vpass)) {
+     446             :         // At least one sequence has passed the convergence test.
+     447             :         // Store variables before iterating
+     448             : 
+     449        1106 :         for (i = 0; i < N; i++)
+     450         976 :           svk[i] = K[i];
+     451             : 
+     452         130 :         s = ss;
+     453             : 
+     454             :         // Choose iteration according to the fastest converging sequence
+     455             : 
+     456         130 :         stry = vtry = 0;
+     457         130 :         fflag       = 1;
+     458             : 
+     459         137 :         do {
+     460         137 :           iFlag = 1;  // Begin each loop by assuming RealIT will be called
+     461             :                       // UNLESS iFlag changed below
+     462             : 
+     463         137 :           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          74 :             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          74 :             if ((*NZ) > 0)
+     471             :               return;
+     472             : 
+     473             :             // Quadratic iteration has failed. Flag that it has been tried and
+     474             :             // decrease the
+     475             :             // convergence criterion
+     476             : 
+     477          48 :             vtry = 1;
+     478          48 :             betav *= 0.25;
+     479             : 
+     480             :             // Try linear iteration if it has not been tried and the s sequence
+     481             :             // is converging
+     482          48 :             if (stry || (!spass)) {
+     483          28 :               iFlag = 0;
+     484             :             }  // End if (stry || (!spass))
+     485             :             else {
+     486         177 :               for (i = 0; i < N; i++)
+     487         157 :                 K[i] = svk[i];
+     488             :             }  // End if (stry || !spass)
+     489             : 
+     490             :           }  // End else !fflag
+     491             : 
+     492         111 :           if (iFlag != 0) {
+     493          83 :             RealIT_ak1(&iFlag, NZ, &s, N, p, NN, qp, szr, szi, K, qk);
+     494             : 
+     495          83 :             if ((*NZ) > 0)
+     496             :               return;
+     497             : 
+     498             :             // Linear iteration has failed. Flag that it has been tried and
+     499             :             // decrease the
+     500             :             // convergence criterion
+     501             : 
+     502          27 :             stry = 1;
+     503          27 :             betas *= 0.25;
+     504             : 
+     505          27 :             if (iFlag != 0) {
+     506             :               // If linear iteration signals an almost double real zero, attempt
+     507             :               // quadratic iteration
+     508             : 
+     509           7 :               ui = -(s + s);
+     510           7 :               vi = s * s;
+     511           7 :               continue;
+     512             : 
+     513             :             }  // End if (iFlag != 0)
+     514             :           }    // End if (iFlag != 0)
+     515             : 
+     516             :           // Restore variables
+     517         476 :           for (i = 0; i < N; i++)
+     518         428 :             K[i] = svk[i];
+     519             : 
+     520             :           // Try quadratic iteration if it has not been tried and the v sequence
+     521             :           // is converging
+     522             : 
+     523          55 :         } while (vpass && !vtry);  // End do-while loop
+     524             : 
+     525             :         // Re-compute qp and scalar values to continue the second stage
+     526             : 
+     527          48 :         QuadSD_ak1(NN, u, v, p, qp, &a, &b);
+     528          48 :         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         188 :     ovv = vv;
+     535         188 :     oss = ss;
+     536         188 :     otv = tv;
+     537         188 :     ots = ts;
+     538             :   }  // End for j
+     539             : 
+     540             :   return;
+     541             : }  // End Fxshfr_ak1
+     542             : 
+     543         776 : 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         776 :   int i;
+     548             : 
+     549         776 :   q[0] = *b = p[0];
+     550         776 :   q[1] = *a = -((*b) * u) + p[1];
+     551             : 
+     552        5769 :   for (i = 2; i < NN; i++) {
+     553        4993 :     q[i] = -((*a) * u + (*b) * v) + p[i];
+     554        4993 :     *b   = (*a);
+     555        4993 :     *a   = q[i];
+     556             :   }  // End for i
+     557             : 
+     558         776 :   return;
+     559             : }  // End QuadSD_ak1
+     560             : 
+     561         546 : 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         546 :   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         546 :   QuadSD_ak1(N, u, v, K, qk, c, d);
+     575             : 
+     576         546 :   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             :       return dumFlag;
+     579             :   }  // End if (fabs(c) <= (100.0*DBL_EPSILON*fabs(K[N - 1])))
+     580             : 
+     581         546 :   *h = v * b;
+     582         546 :   if (fabs((*d)) >= fabs((*c))) {
+     583         316 :     dumFlag = 2;  // TYPE = 2 indicates that all formulas are divided by d
+     584         316 :     *e      = a / (*d);
+     585         316 :     *f      = (*c) / (*d);
+     586         316 :     *g      = u * b;
+     587         316 :     *a3     = (*e) * ((*g) + a) + (*h) * (b / (*d));
+     588         316 :     *a1     = -a + (*f) * b;
+     589         316 :     *a7     = (*h) + ((*f) + u) * a;
+     590             :   }  // End if(fabs(d) >= fabs(c))
+     591             :   else {
+     592         230 :     dumFlag = 1;  // TYPE = 1 indicates that all formulas are divided by c;
+     593         230 :     *e      = a / (*c);
+     594         230 :     *f      = (*d) / (*c);
+     595         230 :     *g      = (*e) * u;
+     596         230 :     *a3     = (*e) * a + ((*g) + (*h) / (*c)) * b;
+     597         230 :     *a1     = -(a * ((*d) / (*c))) + b;
+     598         230 :     *a7     = (*g) * (*d) + (*h) * (*f) + a;
+     599             :   }  // End else
+     600             : 
+     601             :   return dumFlag;
+     602             : }  // End calcSC_ak1
+     603             : 
+     604         342 : 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         342 :   int    i;
+     608         342 :   double temp;
+     609             : 
+     610         342 :   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             :     return;
+     617             :   }  // End if (tFlag == 3)
+     618             : 
+     619         342 :   temp = ((tFlag == 1) ? b : a);
+     620             : 
+     621         342 :   if (fabs(a1) > (10.0 * DBL_EPSILON * fabs(temp))) {
+     622             :     // Use scaled form of the recurrence
+     623             : 
+     624         342 :     (*a7) /= a1;
+     625         342 :     (*a3) /= a1;
+     626         342 :     K[0] = qp[0];
+     627         342 :     K[1] = -((*a7) * qp[0]) + qp[1];
+     628             : 
+     629        2515 :     for (i = 2; i < N; i++)
+     630        2173 :       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             :   return;
+     644             : 
+     645             : }  // End nextK_ak1
+     646             : 
+     647         342 : 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         342 :   double a4, a5, b1, b2, c1, c2, c3, c4, temp;
+     653             : 
+     654         342 :   (*vv) = (*uu) = 0.0;  // The quadratic is zeroed
+     655             : 
+     656         342 :   if (tFlag != 3) {
+     657         342 :     if (tFlag != 2) {
+     658         134 :       a4 = a + u * b + h * f;
+     659         134 :       a5 = c + (u + v * f) * d;
+     660             :     }       // End if (tFlag != 2)
+     661             :     else {  // else tFlag == 2
+     662         208 :       a4 = (a + g) * f + h;
+     663         208 :       a5 = (f + u) * c + v * d;
+     664             :     }  // End else tFlag == 2
+     665             : 
+     666             :     // Evaluate new quadratic coefficients
+     667             : 
+     668         342 :     b1   = -K[N - 1] / p[N];
+     669         342 :     b2   = -(K[N - 2] + b1 * p[N - 1]) / p[N];
+     670         342 :     c1   = v * b2 * a1;
+     671         342 :     c2   = b1 * a7;
+     672         342 :     c3   = b1 * b1 * a3;
+     673         342 :     c4   = -(c2 + c3) + c1;
+     674         342 :     temp = -c4 + a5 + b1 * a4;
+     675         342 :     if (temp != 0.0) {
+     676         342 :       *uu = -((u * (c3 + c2) + v * (b1 * a1 + b2 * a7)) / temp) + u;
+     677         342 :       *vv = v * (1.0 + c4 / temp);
+     678             :     }  // End if (temp != 0)
+     679             : 
+     680             :   }  // End if (tFlag != 3)
+     681             : 
+     682         342 :   return;
+     683             : }  // End newest_ak1
+     684             : 
+     685          74 : 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          74 :   int    i, j = 0, tFlag, triedFlag = 0;
+     692          74 :   double c, ee, mp, omp, relstp, t, u, ui, v, vi, zm;
+     693             : 
+     694          74 :   *NZ = 0;   // Number of zeros found
+     695          74 :   u   = uu;  // uu and vv are coefficients of the starting quadratic
+     696          74 :   v   = vv;
+     697             : 
+     698         146 :   do {
+     699         146 :     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         146 :     if (fabs(fabs(*szr) - fabs(*lzr)) > 0.01 * fabs(*lzr))
+     706             :       break;
+     707             : 
+     708             :     // Evaluate polynomial by quadratic synthetic division
+     709             : 
+     710          98 :     QuadSD_ak1(NN, u, v, p, qp, a, b);
+     711             : 
+     712          98 :     mp = fabs(-((*szr) * (*b)) + (*a)) + fabs((*szi) * (*b));
+     713             : 
+     714             :     // Compute a rigorous bound on the rounding error in evaluating p
+     715             : 
+     716          98 :     zm = sqrt(fabs(v));
+     717          98 :     ee = 2.0 * fabs(qp[0]);
+     718          98 :     t  = -((*szr) * (*b));
+     719             : 
+     720         822 :     for (i = 1; i < N; i++)
+     721         724 :       ee = ee * zm + fabs(qp[i]);
+     722             : 
+     723          98 :     ee = ee * zm + fabs((*a) + t);
+     724          98 :     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          98 :     if (mp <= 20.0 * ee) {
+     730          26 :       *NZ = 2;
+     731          26 :       break;
+     732             :     }  // End if (mp <= 20.0*ee)
+     733             : 
+     734          72 :     j++;
+     735             : 
+     736             :     // Stop iteration after 20 steps
+     737          72 :     if (j > 20)
+     738             :       break;
+     739             : 
+     740          72 :     if (j >= 2) {
+     741          35 :       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           0 :         relstp = ((relstp < DBL_EPSILON) ? sqrt(DBL_EPSILON) : sqrt(relstp));
+     746             : 
+     747           0 :         u -= u * relstp;
+     748           0 :         v += v * relstp;
+     749             : 
+     750           0 :         QuadSD_ak1(NN, u, v, p, qp, a, b);
+     751             : 
+     752           0 :         for (i = 0; i < 5; i++) {
+     753           0 :           tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, &c, d, e, f, g, h, K, u, v, qk);
+     754           0 :           nextK_ak1(N, tFlag, *a, *b, *a1, a3, a7, K, qk, qp);
+     755             :         }  // End for i
+     756             : 
+     757             :         triedFlag = 1;
+     758             :         j         = 0;
+     759             : 
+     760             :       }  // End if ((relstp <= 0.01) && (mp >= omp) && (!triedFlag))
+     761             : 
+     762             :     }  // End if (j >= 2)
+     763             : 
+     764          72 :     omp = mp;
+     765             : 
+     766             :     // Calculate next K polynomial and new u and v
+     767             : 
+     768          72 :     tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, &c, d, e, f, g, h, K, u, v, qk);
+     769          72 :     nextK_ak1(N, tFlag, *a, *b, *a1, a3, a7, K, qk, qp);
+     770          72 :     tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, &c, d, e, f, g, h, K, u, v, qk);
+     771          72 :     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          72 :     if (vi != 0) {
+     775          72 :       relstp = fabs((-v + vi) / vi);
+     776          72 :       u      = ui;
+     777          72 :       v      = vi;
+     778             :     }                 // End if (vi != 0)
+     779          72 :   } while (vi != 0);  // End do-while loop
+     780             : 
+     781          74 :   return;
+     782             : 
+     783             : }  // End QuadIT_ak1
+     784             : 
+     785          83 : 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          83 :   int    i, j = 0, nm1 = N - 1;
+     793          83 :   double ee, kv, mp, ms, omp, pv, s, t;
+     794             : 
+     795          83 :   *iFlag = *NZ = 0;
+     796          83 :   s            = *sss;
+     797             : 
+     798         817 :   for (;;) {
+     799         450 :     qp[0] = pv = p[0];
+     800             : 
+     801             :     // Evaluate p at s
+     802        3904 :     for (i = 1; i < NN; i++)
+     803        3454 :       qp[i] = pv = pv * s + p[i];
+     804             : 
+     805         450 :     mp = fabs(pv);
+     806             : 
+     807             :     // Compute a rigorous bound on the error in evaluating p
+     808             : 
+     809         450 :     ms = fabs(s);
+     810         450 :     ee = 0.5 * fabs(qp[0]);
+     811        3904 :     for (i = 1; i < NN; i++)
+     812        3454 :       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         450 :     if (mp <= 20.0 * DBL_EPSILON * (2.0 * ee - mp)) {
+     818          56 :       *NZ  = 1;
+     819          56 :       *szr = s;
+     820          56 :       *szi = 0.0;
+     821          56 :       break;
+     822             :     }  // End if (mp <= 20.0*DBL_EPSILON*(2.0*ee - mp))
+     823             : 
+     824         394 :     j++;
+     825             : 
+     826             :     // Stop iteration after 10 steps
+     827             : 
+     828         394 :     if (j > 10)
+     829             :       break;
+     830             : 
+     831         374 :     if (j >= 2) {
+     832         297 :       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           7 :         *iFlag = 1;
+     837           7 :         *sss   = s;
+     838           7 :         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         367 :     omp = mp;
+     846             : 
+     847             :     // Compute t, the next polynomial and the new iterate
+     848         367 :     qk[0] = kv = K[0];
+     849        2875 :     for (i = 1; i < N; i++)
+     850        2508 :       qk[i] = kv = kv * s + K[i];
+     851             : 
+     852         367 :     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         367 :       t    = -(pv / kv);
+     856         367 :       K[0] = qp[0];
+     857        2875 :       for (i = 1; i < N; i++)
+     858        2508 :         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         367 :     kv = K[0];
+     868        2875 :     for (i = 1; i < N; i++)
+     869        2508 :       kv = kv * s + K[i];
+     870             : 
+     871         367 :     t = ((fabs(kv) > (fabs(K[nm1]) * 10.0 * DBL_EPSILON)) ? -(pv / kv) : 0.0);
+     872             : 
+     873         367 :     s += t;
+     874             : 
+     875             :   }  // End infinite for loop
+     876             : 
+     877          83 :   return;
+     878             : 
+     879             : }  // End RealIT_ak1
+     880             : 
+     881         164 : 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         164 :   double b, d, e;
+     890             : 
+     891         164 :   *sr = *si = *lr = *li = 0.0;
+     892             : 
+     893         164 :   if (a == 0) {
+     894           0 :     *sr = ((b1 != 0) ? -(c / b1) : *sr);
+     895           0 :     return;
+     896             :   }  // End if (a == 0))
+     897             : 
+     898         164 :   if (c == 0) {
+     899           0 :     *lr = -(b1 / a);
+     900           0 :     return;
+     901             :   }  // End if (c == 0)
+     902             : 
+     903             :   // Compute discriminant avoiding overflow
+     904             : 
+     905         164 :   b = b1 / 2.0;
+     906         164 :   if (fabs(b) < fabs(c)) {
+     907         114 :     e = ((c >= 0) ? a : -a);
+     908         114 :     e = -e + b * (b / fabs(c));
+     909         114 :     d = sqrt(fabs(e)) * sqrt(fabs(c));
+     910             :   }       // End if (fabs(b) < fabs(c))
+     911             :   else {  // Else (fabs(b) >= fabs(c))
+     912          50 :     e = -((a / b) * (c / b)) + 1.0;
+     913          50 :     d = sqrt(fabs(e)) * (fabs(b));
+     914             :   }  // End else (fabs(b) >= fabs(c))
+     915             : 
+     916         164 :   if (e >= 0) {
+     917             :     // Real zeros
+     918             : 
+     919          78 :     d   = ((b >= 0) ? -d : d);
+     920          78 :     *lr = (-b + d) / a;
+     921          78 :     *sr = ((*lr != 0) ? (c / (*lr)) / a : *sr);
+     922             :   }       // End if (e >= 0)
+     923             :   else {  // Else (e < 0)
+     924             :     // Complex conjugate zeros
+     925             : 
+     926          86 :     *lr = *sr = -(b / a);
+     927          86 :     *si       = fabs(d / a);
+     928          86 :     *li       = -(*si);
+     929             :   }  // End else (e < 0)
+     930             : 
+     931             :   return;
+     932             : }  // End Quad_ak1
+     933             : 
+     934             : 
+     935             : }  // namespace rpoly_impl
+     936             : 
+     937             : //}
+     938             : 
+     939          18 : void rpolyWrapper(double* coefficients_decreasing, int* degree, double* roots_real, double* roots_imag) {
+     940          18 :   rpoly_impl::rpoly_ak1(coefficients_decreasing, degree, roots_real, roots_imag);
+     941          18 : }
+     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..77abb370f095bf05da6d6c9f363ca9cff798f9f5 GIT binary patch literal 4823 zcmV;|5-9D7P)ze9D_vSS?AyDGJ7YJjKV__dPAmVUP_yf z=b2xX=Q$;M@p_u334oK=3gBEmn@V5hDWUgS`Z$16pu;HuB16+Gd0>61V<(woqQ53@ zF{%6+dOQdYg_#E(A#P^jwls7z@^3O&>87wysG z3tlzx@oYMK+cV5PEDPIQF~H$MY#BN!o{FqW%;EgHE22aIl+ULk%9CYC-b+gXWMaB= zR9SgXQ>KjqMtgK83Bd3i?_0KV{9F5?$7K6yP4-6GRYPO&6jI$zk284GVGWDLYW3Jh z5k>GewDy?3UdlA>dADujXMQ*5mD{w|kU8)2x2jE-?_L2;DwtB2yUNhpw&^Ul*YMlx zCs%WL4ZUyGBrwpT8gA zgq$q5}Y~mesvc! zAOLddbX~FJYdKXAo`D;KowJN?0-o1402pknKJp>Ag@9-I%{^Xn%mrXj?!_6r(g~ZO zAUD{o&tlbLP90nVl8(z|-Fd&3D5|+GHWc2;3>d@DAJ|+g@aOD?zW@xjK@!IH^-}yp zw?T$`5;`u!|G;Yg2P^FbT0+U}%sAi(*ADJ_|)c6i~xJ z8e@bZpUftpkM=?&RgV&AQm25QU;HS5vwe^dDL>kw>)y`OkfAj!X8TTKb&J`d{~YBvVroZZxr+} z;7f6z44wm!)|ht%%mkR<%#*jFGxb>PuDrc{zybVh)tfO=u1%Q&c<93C59!0A6Zw~) z;tc7V6KT3%ym7RK(ma5EegfL{;ksK;!><|ggq|HAExu+L zCSbS~%50lUnnB8F@~Ul86*q;Plnxewo~<8n8Ji=~x=@;=a&KsM=tY{h^i1h5u(=Re z?2cxN!ic5Dm3e?2bvT0SxwSUwj8R4;94%I5$vY55r zhD~N8%CMOe;BX(*FhySgVb5%|Ww&O>Lp>6gIs0`8mCK9-WP&+?%z1z|flfT-Xdiz7`MY(e(;-j**0=*!@y{@fBYdQir)G&_srl@l9qri(Pr^H6n$O zDL|T;f3MX$Z3T^_9%0SPpbqs2Q=zjO)e2j04+@9-dXv$^>+|qPdjaD&0H-7PAw>js?fFp;tg>GR6)$&FI@YV_?6se59Oyuirw#B9!ya_U3+R+YfH8+bm2Wp$*i{ zvk5W=YUM6zy+h;Wb~pu;$?sXrD?m`@DhXHNk(|wLr!`t_EXG}qS#^MMHPqJ! zRF}O6I7C8UCw!(Aq}*fNiwDD(6e1I>dUlaBmsg_#L(s3GX$tAItzqh)uO zi1Dl?$AvyQx9jj~T4Teu#XU1_UOgi~-IR?$TicB1=u3OV0Nf1=Krvt835pHN8?%Z@ z^*SA*VB)IkF}iz{76UT1cw-%txOUVGDn)%#tN>z6$*dTera0d3g}S=fH46OXo5!# zfz839_AYu%$e8k}On8YYZzGj}tArJOz!e@Q(l4BU=5ck^a%jPihpv zN2N;oR4uK<-;IKFvud)1pH$XBhHfdXDDRa2KgMc)Xn}gyyr#a~8ukL%1p*+<@V>7w z&=SsP&Nop}x8Pg#M%~o@@57x-V#ryM%9ui&=fk*CfNG&sN@9G3bEQk+gPs=`Z&78I zM4AkA-ffbv7Sn?(zcy54QSFx)AA6_nnj{4x@HiuC3IkC%K_?YyR|h7kVL}NaC;Lu< z?K+eM#Qg5TC3qXN>H{K=dB@nt2?(SJvYK-E%?Nwgc3I$%dd?{2J0A2<)z(k?jFh?^ zRdXRga?E_^jQ^bbwMsNJ0eZnJ%1Ont0FkDrccJZ<#z{u0Aju%m*KsrT|4~aS1*IF# z@gO%G>@iYJ!8LK5bl}QH5+i5yPjGv^q*O8CLc4vj^S++^WUk3f&AL3?x0C(Lwo z3;uTJNQ4Rih89zL43rj_Q2dIbScKy>sdO6Jm~ih_&=`8O^onE2*)aV^(g7)rEgPA; zVS;7?)U8%Z-R0j$Qa+3s@l?cE*f)7Ss@MhBU9Lcn{XohPWq8?Iz^Dh2G`y*&i`axgntyLX| zIr?W_F|;S|jU(RAc3;l%vbcDxC1KGUELd8Je|)DB0IY>2Ly}E(=RYO@aGG`D_-2BE zX!ra7Hvzysx?CZY*$}Eyb;s(J-l_3(BYLT4kH;hyE9cOXG9h+siBJfuIz_e zcMcwhQyTZ72Z>2!WM;Dgmt?HcfSQT7V|+dAwl@nk$IWXot4}8|#$%0yZa*vq^V*Hi z3@t{u7LVVe937U!QcLexWfVPbQ325N-vNe>0u5=d;Y`JCS-?6|A%P@~6!EiOTEhMd zj=Uh8l=;MpUPv0dktEO#N#>Ge9gO34ipQ=19r(Wq`0^=)oO{C`fNP+Fbp-Hs4)^Um z?onj6S8y6`dsVQf5B5Ta;}Il0Cw<-=8!3{M6La`QZ?oVy^hj-wt<{%GxA-zIdL&IN z+6uGOvx9oGb7Z7N196{E%QoE0RiZeV&q9fKL0B^9(bNzWoGB7H=LIC_v2jg|*@>;-DgS!ay@>CGLZyn- z$YJpwW2Veu$c>LjGwpTYE1*kXFd)g>(n3x+hh)=v~I;PGMvWl z3i0^Rqt3#Okq5*Hx`&X)NeJn<$@stWCi~ z8l3XQ*jWYYS-L2>A8prRNg?8b zt;2KmsT4}OL&w`4k)W#mX+S4w}RT|K}cBo_jq^%D?${F-i7=m6#wB~W{((0&S6K(BfmfO`;8 z0i8L@SwQ!Os$fIv%5dy={78Wzz67DWYKkD?!h}`<6a)H4;~CHoyegnmIA|8*-Uq-X zeNLy7Cb|8CU+_B0Kht~V*f3zz!L&WvOmk=Gc>4>p@IQ;~saDva0j$r%IwDtE!6=cl zCfW6_Xw?J@Y9nl+6`w(hOj5OpoKE@_)zrx6$~HiGZ48^zbmVY1e&ft$u{P0N5~5g} zF@vL!oNh|l)L`G$PZJ(sz{t??IYzFhK}-r!KqS{%xwue|_6UT?=OjT_&KyeEb%cFz zG~+wgnWhPa^#{JEKPnxfNMCLo-)Sg*&un zAhiBLC=QY*{(8`aEg~uh8A;k%2%>>vhg~2j23C% zEFag{rNE~;dq#bhAaD)HvO@H~*4dNF%M$PvLxryAHA*bL1Z|H8<*%NS-@+Z{_P3v* zSjyv3uFi2z@Qwk7hsMm0Q^w^;?DsvI&`|Tz8>xMW?*OE`egi#50flpb zk;{0~#5YtPHn1dmzdM{3cJ0GG{$cV{s*~C2xAES6Ghdx3+K>KnzR`bR?8B9ZW7hx9 zX!>q=|9pBeOysg;d@(V9S?Nc&a%ix?2~_SjDBa|;LGB4$>$>99xm%kyR%I7FO1ArP z&!M69zII0#uT?2ve`xVYZYn~A1L*?>d%SjXxw`u!t%Ny7(y37=cL3q{x}^Ujej)qA z2uZ)MItRbo#jWkJfDsaY=xL&z2N^=of8}Wok?>z}NwoknGoiMoOPWPt>yt>OGCYUe zZ3y&>(?;vcR$Sm}-h(ZofU#JLUI8!6yOBL-WctW*{^WaVPEzT+YUDq2eC&ON3+~!X zr=g>YeWZ5gb~TL1@?OQ^-@aUaMUjB5fWl4TQdtTb`lMRR&@9 zNrJsl($?cv$3qixfwFh`dgo(osDJ;PiD#T~(fbVEj({(dCLrBqFSAiJkq>x{lkqM~ z4kqK}pL=IPsK=18S4yf&apDhOre7ajsifK1H=RBkf%Q+s-socb1An-fyCUq19Q%d{ z`t}|q_5veJA==kTERxCZUHC_U@&zm1E10YfsFeN*u(A}|af8}Ym^;ZW8rl@PwNHqh z=Ga_}76P&WZP-0=5=J?E + + + + + + 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:5212740.9 %
Date:2023-12-06 07:59:45Functions: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::operator[](unsigned long) const4
eth_trajectory_generation::Segment::operator[](unsigned long)12
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> >*) const18
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*) const18
eth_trajectory_generation::Segment::computeMinMaxMagnitudeCandidateTimes(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<double, std::allocator<double> >*) const18
eth_trajectory_generation::Segment::evaluate(double, int) const200
+
+
+ + + +
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..2ade9e1ead --- /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:5212740.9 %
Date:2023-12-06 07:59:45Functions: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)12
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> >*) const18
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*) const18
eth_trajectory_generation::Segment::computeMinMaxMagnitudeCandidateTimes(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<double, std::allocator<double> >*) const18
eth_trajectory_generation::Segment::evaluate(double, int) const200
eth_trajectory_generation::Segment::operator==(eth_trajectory_generation::Segment const&) const0
eth_trajectory_generation::Segment::operator[](unsigned long) const4
+
+
+ + + +
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..af41423150 --- /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:5212740.9 %
Date:2023-12-06 07:59:45Functions: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             :     return false;
+      34             :   } else {
+      35           0 :     for (int i = 0; i < D(); i++) {
+      36           0 :       if (polynomials_[i] != rhs[i]) {
+      37             :         return false;
+      38             :       }
+      39             :     }
+      40             :   }
+      41             :   return true;
+      42             : }
+      43             : 
+      44             : //}
+      45             : 
+      46             : /* Segment::operator[](size_t idx) //{ */
+      47             : 
+      48          12 : Polynomial& Segment::operator[](size_t idx) {
+      49          12 :   CHECK_LT(idx, static_cast<size_t>(D_));
+      50          12 :   return polynomials_[idx];
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* Segment::operator[](size_t idx) //{ */
+      56             : 
+      57           4 : const Polynomial& Segment::operator[](size_t idx) const {
+      58           4 :   CHECK_LT(idx, static_cast<size_t>(D_));
+      59           4 :   return polynomials_[idx];
+      60             : }
+      61             : 
+      62             : //}
+      63             : 
+      64             : /* evaluate() //{ */
+      65             : 
+      66         200 : Eigen::VectorXd Segment::evaluate(double t, int derivative) const {
+      67         200 :   Eigen::VectorXd result(D_);
+      68         200 :   result.setZero();
+      69        1000 :   for (int d = 0; d < D_; ++d) {
+      70         800 :     result[d] = polynomials_[d].evaluate(t, derivative);
+      71             :   }
+      72         200 :   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          18 : 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          18 :   CHECK_NOTNULL(candidate_times);
+     117          18 :   candidate_times->clear();
+     118             :   // Compute magnitude derivative roots.
+     119          18 :   if (dimensions.empty()) {
+     120           0 :     LOG(WARNING) << "No dimensions specified." << std::endl;
+     121           0 :     return false;
+     122          18 :   } else if (dimensions.size() > 1) {
+     123           6 :     const int       n_d                           = N_ - derivative;
+     124           6 :     const int       n_dd                          = n_d - 1;
+     125           6 :     const int       convolved_coefficients_length = Polynomial::getConvolutionLength(n_d, n_dd);
+     126           6 :     Eigen::VectorXd convolved_coefficients(convolved_coefficients_length);
+     127           6 :     convolved_coefficients.setZero();
+     128          18 :     for (int dim : dimensions) {
+     129          12 :       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          24 :       Eigen::VectorXd d  = polynomials_[dim].getCoefficients(derivative).head(n_d);
+     137          36 :       Eigen::VectorXd dd = polynomials_[dim].getCoefficients(derivative + 1).head(n_dd);
+     138          24 :       convolved_coefficients += Polynomial::convolve(d, dd);
+     139             :     }
+     140          12 :     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           6 :     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          12 :     if (!polynomials_[dimensions[0]].computeMinMaxCandidates(t_start, t_end, derivative, candidate_times)) {
+     152           0 :       return false;
+     153             :     }
+     154             :   }
+     155             :   return true;
+     156             : }
+     157             : 
+     158             : //}
+     159             : 
+     160             : /* computeMinMaxMagnitudeCandidates() //{ */
+     161             : 
+     162          18 : bool Segment::computeMinMaxMagnitudeCandidates(
+     163             : 
+     164             :     int derivative, double t_start, double t_end, const std::vector<int>& dimensions, std::vector<Extremum>* candidates) const {
+     165          18 :   CHECK_NOTNULL(candidates);
+     166             :   // Find candidate times (roots + start + end).
+     167          18 :   std::vector<double> candidate_times;
+     168          18 :   computeMinMaxMagnitudeCandidateTimes(derivative, t_start, t_end, dimensions, &candidate_times);
+     169             : 
+     170             :   // Evaluate candidate times.
+     171          18 :   candidates->resize(candidate_times.size());
+     172         122 :   for (size_t i = 0; i < candidate_times.size(); i++) {
+     173         104 :     double magnitude = 0.0;
+     174         246 :     for (int dim : dimensions) {
+     175         142 :       magnitude += std::pow(polynomials_[dim].evaluate(candidate_times[i], derivative), 2);
+     176             :     }
+     177         104 :     magnitude        = std::sqrt(magnitude);
+     178         104 :     (*candidates)[i] = Extremum(candidate_times[i], magnitude, 0);
+     179             :   }
+     180             : 
+     181          36 :   return true;
+     182             : }
+     183             : 
+     184             : //}
+     185             : 
+     186             : /* selectMinMaxMagnitudeFromCandidates() //{ */
+     187             : 
+     188          18 : 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          18 :   CHECK_NOTNULL(minimum);
+     193          18 :   CHECK_NOTNULL(maximum);
+     194          18 :   if (t_start > t_end) {
+     195           0 :     LOG(WARNING) << "t_start is greater than t_end.";
+     196           0 :     return false;
+     197             :   }
+     198             : 
+     199          18 :   minimum->value = std::numeric_limits<double>::max();
+     200          18 :   maximum->value = std::numeric_limits<double>::lowest();
+     201             : 
+     202             :   // Evaluate passed candidates.
+     203         122 :   for (const Extremum& candidate : candidates) {
+     204         104 :     if (candidate.time < t_start || candidate.time > t_end) {
+     205           0 :       continue;
+     206             :     }
+     207         104 :     *maximum = std::max(*maximum, candidate);
+     208         132 :     *minimum = std::min(*minimum, candidate);
+     209             :   }
+     210             : 
+     211             :   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             :     }
+     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             :           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             :   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..40fd4130ce0ea43a66ba68b929f4f1ccaac06d09 GIT binary patch literal 1506 zcmV<81s(c{P)GAU`EmxJcx zFa9&}!QgXxElU%K;0CS&!guolHVclwmn?o#Q_$57pUSg&aSoM)3^yN z{NR9UdN5dML=pXv;@_L(70JFhZ7*xrnx^%2hMr*v+(rlj4!)B>y?XwH!fjjUFRAwb zwZh-d$BbSLJ#MyGfwWEvr*Hb|Sry7g;4#*=ZPupwwmvWoX{Ar#lLRVln4RJh_;;M* zNw=uh>j1B?!@?YPT9zl5m-LWV>%FnwC1Ct1qF|BV_r{`$gJGfI5h&^Sa1zV)c)iB? zd}1;)+6SH{ldMkes_G!15(lM%3i#n1j$1O?Wce}RD9SZ zZY8rle#A-;4t!O>qjYJHC>1XYoY%C)#5qSFywB7I6pcXo82V+VL`2XL`{7z_XYJOB zTr^>^VF#GZ&Vb1<+~W2ny+>D*l2dfJleX54nD-whw*nW|KAf2{QNMFBh-moExnBss zy3CL3+7pC?ggwio^s?w?x|K%XL1ONhWXDHC0;h_KOMNN!h1B312jnAC>qP$M4$>2w zs!jl&V))|=`zTXDg9Y~c(V;eYHGtgj`oo@T3LdEFv+3!E)Foq^VdS)@Zno?qBWZ~f znYnK8K_Z1}W*gT%cUIS=h>M(dH`zxpm!%6tVT6Wo(hSu0Qq+WMydLo{HC;Hp*Zov1 z)nr1odX$-h9XHLx#f}Hp$VzvBDJ9rZydZ zwz-wW^5aEmv6lS1F&0kB=fy|D2_S7L!;Vd?)c)P^#Mu940&ux|iB0kj6Q%VTMB8ov z>;MGNU-#GkYJ=_Flf{ld(y2y%!Yc^zpdh{t)RXaECR?>0_3j0?RBF?e^TOE=~dOb@6$c!hgXp|hITJZ8r$EHuM);9S=u zg2sXbYJ1J + + + + + + 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:2023-12-06 07:59:45Functions: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..5f3fc085aa --- /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:2023-12-06 07:59:45Functions: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..a32cca9282 --- /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:2023-12-06 07:59:45Functions: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           0 :   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             :     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..2f4fd270567baa761be463cdc93a69a9924d1442 GIT binary patch literal 1087 zcmV-F1i<@=P)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_ziXY0#Il&{m7m%0FO68#Za216)=!d`uP#&s`g6i78g0!zR57Q<}x_?#_SWE_6- zS~JEnWwdYDE5_XwZLug%prz*PQVg~`YiKA6Sc)=4%3_F`W`#8f&Y%yo>Bf*$xB9>-zEu?T+Q(f~-PRI6d93_!UE$#y?lVw!SVUm7maD<@MT36RGXx zOaZ6KQN0WM + + + + + + 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:17829859.7 %
Date:2023-12-06 07:59:45Functions: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)1
eth_trajectory_generation::Trajectory::getSegmentTimes() const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*, int) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*, int) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*, int) const1
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> >*) const5
eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*) const9
eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*, int) const9
+
+
+ + + +
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..8673f3d21c --- /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:17829859.7 %
Date:2023-12-06 07:59:45Functions: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)1
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> >*) const5
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() const1
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*) const9
eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*, int) const9
eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*, int) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*, int) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*) const1
eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*, int) const1
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..eef8dd30b3 --- /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:17829859.7 %
Date:2023-12-06 07:59:45Functions: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             :     return false;
+      41             :   } else {
+      42           0 :     for (int i = 0; i < K(); i++) {
+      43           0 :       if (segments_ != rhs.segments_) {
+      44             :         return false;
+      45             :       }
+      46             :     }
+      47             :     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             :       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           5 : 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           5 :   const size_t expected_number_of_samples = (t_end - t_start) / dt + 1;
+      96             : 
+      97           5 :   result->clear();
+      98           5 :   result->reserve(expected_number_of_samples);
+      99             : 
+     100           5 :   if (sampling_times != nullptr) {
+     101           0 :     sampling_times->clear();
+     102           0 :     sampling_times->reserve(expected_number_of_samples);
+     103             :   }
+     104             : 
+     105           5 :   double accumulated_time = 0.0;
+     106             : 
+     107             :   // Look for the correct segment to start.
+     108           5 :   size_t i = 0;
+     109           5 :   for (i = 0; i < segments_.size(); ++i) {
+     110           5 :     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           5 :     if (accumulated_time > t_start) {
+     118             :       break;
+     119             :     }
+     120             :   }
+     121           5 :   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           5 :   accumulated_time -= segments_[i].getTime();
+     128           5 :   double time_in_segment = t_start - accumulated_time;
+     129             : 
+     130             :   // Get all the samples, incrementing the segments as we go.
+     131         205 :   while (accumulated_time < t_end) {
+     132         200 :     if (time_in_segment > segments_[i].getTime()) {
+     133           0 :       time_in_segment = time_in_segment - segments_[i].getTime();
+     134           0 :       i++;
+     135             :       // Make sure we don't access segments that don't exist!
+     136           0 :       if (i >= segments_.size()) {
+     137             :         break;
+     138             :       }
+     139           0 :       continue;
+     140             :     }
+     141             : 
+     142         400 :     result->push_back(segments_[i].evaluate(time_in_segment, derivative_order));
+     143             : 
+     144         200 :     if (sampling_times != nullptr) {
+     145           0 :       sampling_times->push_back(accumulated_time);
+     146             :     }
+     147             : 
+     148         200 :     time_in_segment += dt;
+     149         200 :     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             :   return true;
+     205             : }
+     206             : 
+     207             : //}
+     208             : 
+     209             : /* computeMinMaxMagnitude() //{ */
+     210             : 
+     211           9 : bool Trajectory::computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum, int seg) const {
+     212             : 
+     213           9 :   CHECK_NOTNULL(minimum);
+     214           9 :   CHECK_NOTNULL(maximum);
+     215             : 
+     216           9 :   minimum->value = std::numeric_limits<double>::max();
+     217           9 :   maximum->value = std::numeric_limits<double>::lowest();
+     218             : 
+     219             :   // Compute candidates.
+     220          18 :   std::vector<Extremum> candidates;
+     221           9 :   if (!segments_[seg].computeMinMaxMagnitudeCandidates(derivative, 0.0, segments_[seg].getTime(), dimensions, &candidates)) {
+     222             :     return false;
+     223             :   }
+     224             :   // Evaluate candidates.
+     225           9 :   Extremum minimum_candidate, maximum_candidate;
+     226           9 :   if (!segments_[seg].selectMinMaxMagnitudeFromCandidates(derivative, 0.0, segments_[seg].getTime(), dimensions, candidates, &minimum_candidate,
+     227             :                                                           &maximum_candidate)) {
+     228             :     return false;
+     229             :   }
+     230             :   // Select minimum / maximum.
+     231           9 :   if (minimum_candidate < *minimum) {
+     232           9 :     *minimum             = minimum_candidate;
+     233           9 :     minimum->segment_idx = static_cast<int>(seg);
+     234             :   }
+     235           9 :   if (maximum_candidate > *maximum) {
+     236           9 :     *maximum             = maximum_candidate;
+     237           9 :     maximum->segment_idx = static_cast<int>(seg);
+     238             :   }
+     239             : 
+     240             :   return true;
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : /* computeMinMaxMagnitude() //{ */
+     246             : 
+     247           9 : bool Trajectory::computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum) const {
+     248             : 
+     249           9 :   CHECK_NOTNULL(minimum);
+     250           9 :   CHECK_NOTNULL(maximum);
+     251             : 
+     252           9 :   minimum->value = std::numeric_limits<double>::max();
+     253           9 :   maximum->value = std::numeric_limits<double>::lowest();
+     254             : 
+     255             :   // For all segments in the trajectory:
+     256          18 :   for (size_t segment_idx = 0; segment_idx < segments_.size(); segment_idx++) {
+     257             : 
+     258             :     // Compute candidates.
+     259          18 :     std::vector<Extremum> candidates;
+     260           9 :     if (!segments_[segment_idx].computeMinMaxMagnitudeCandidates(derivative, 0.0, segments_[segment_idx].getTime(), dimensions, &candidates)) {
+     261           0 :       return false;
+     262             :     }
+     263             :     // Evaluate candidates.
+     264           9 :     Extremum minimum_candidate, maximum_candidate;
+     265           9 :     if (!segments_[segment_idx].selectMinMaxMagnitudeFromCandidates(derivative, 0.0, segments_[segment_idx].getTime(), dimensions, candidates,
+     266             :                                                                     &minimum_candidate, &maximum_candidate)) {
+     267             :       return false;
+     268             :     }
+     269             :     // Select minimum / maximum.
+     270           9 :     if (minimum_candidate < *minimum) {
+     271           9 :       *minimum             = minimum_candidate;
+     272           9 :       minimum->segment_idx = static_cast<int>(segment_idx);
+     273             :     }
+     274           9 :     if (maximum_candidate > *maximum) {
+     275           9 :       *maximum             = maximum_candidate;
+     276           9 :       maximum->segment_idx = static_cast<int>(segment_idx);
+     277             :     }
+     278             :   }
+     279             :   return true;
+     280             : }
+     281             : 
+     282             : //}
+     283             : 
+     284             : /* getSegmentTimes() //{ */
+     285             : 
+     286           1 : std::vector<double> Trajectory::getSegmentTimes() const {
+     287           1 :   std::vector<double> segment_times(segments_.size());
+     288           2 :   for (size_t i = 0; i < segment_times.size(); ++i) {
+     289           1 :     segment_times[i] = segments_[i].getTime();
+     290             :   }
+     291           1 :   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             :   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             :   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             :     return false;
+     385           0 :   if (!temp_vertex.getSubdimension(kYawDimensions, max_derivative_order_yaw, &yaw_vertices->front()))
+     386             :     return false;
+     387             : 
+     388             :   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             :       return false;
+     394           0 :     if (!temp_vertex.getSubdimension(kYawDimensions, max_derivative_order_yaw, &(*yaw_vertices)[i + 1]))
+     395             :       return false;
+     396             :   }
+     397             :   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           1 : 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           1 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     426             : 
+     427           1 :   dimensions.push_back(0);
+     428           1 :   dimensions.push_back(1);
+     429             : 
+     430           1 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     431             : 
+     432           1 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj, seg);
+     433           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj, seg);
+     434           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj, seg);
+     435             : 
+     436           1 :   *v_max = v_max_traj.value;
+     437           1 :   *a_max = a_max_traj.value;
+     438           1 :   *j_max = j_max_traj.value;
+     439             : 
+     440           2 :   return success;
+     441             : }
+     442             : 
+     443             : //}
+     444             : 
+     445             : /* computeMaxDerivativesHorizontal() //{ */
+     446             : 
+     447             : // compute max velocity, acceleration and jerk
+     448           1 : 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           1 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     452             : 
+     453           1 :   dimensions.push_back(0);
+     454           1 :   dimensions.push_back(1);
+     455             : 
+     456           1 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     457             : 
+     458           1 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj);
+     459           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj);
+     460           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj);
+     461             : 
+     462           1 :   *v_max = v_max_traj.value;
+     463           1 :   *a_max = a_max_traj.value;
+     464           1 :   *j_max = j_max_traj.value;
+     465             : 
+     466           2 :   return success;
+     467             : }
+     468             : 
+     469             : //}
+     470             : 
+     471             : /* computeMaxDerivativesVertical() //{ */
+     472             : 
+     473             : // compute max velocity, acceleration and jerk
+     474           1 : 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           1 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     478             : 
+     479           1 :   dimensions.push_back(2);
+     480             : 
+     481           1 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     482             : 
+     483           1 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj, seg);
+     484           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj, seg);
+     485           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj, seg);
+     486             : 
+     487           1 :   *v_max = v_max_traj.value;
+     488           1 :   *a_max = a_max_traj.value;
+     489           1 :   *j_max = j_max_traj.value;
+     490             : 
+     491           2 :   return success;
+     492             : }
+     493             : 
+     494             : //}
+     495             : 
+     496             : /* computeMaxDerivativesVertical() //{ */
+     497             : 
+     498             : // compute max velocity, acceleration and jerk
+     499           1 : 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           1 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     503             : 
+     504           1 :   dimensions.push_back(2);
+     505             : 
+     506           1 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     507             : 
+     508           1 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj);
+     509           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj);
+     510           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj);
+     511             : 
+     512           1 :   *v_max = v_max_traj.value;
+     513           1 :   *a_max = a_max_traj.value;
+     514           1 :   *j_max = j_max_traj.value;
+     515             : 
+     516           2 :   return success;
+     517             : }
+     518             : 
+     519             : //}
+     520             : 
+     521             : /* computeMaxDerivativesHeading() //{ */
+     522             : 
+     523             : // compute max velocity, acceleration and jerk
+     524           1 : bool Trajectory::computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max, int seg) const {
+     525             : 
+     526           1 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     527             : 
+     528           1 :   dimensions.push_back(3);  // 3 = heading
+     529             : 
+     530           1 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     531             : 
+     532           1 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj, seg);
+     533           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj, seg);
+     534           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj, seg);
+     535             : 
+     536           1 :   *v_max = v_max_traj.value;
+     537           1 :   *a_max = a_max_traj.value;
+     538           1 :   *j_max = j_max_traj.value;
+     539             : 
+     540           2 :   return success;
+     541             : }
+     542             : 
+     543             : //}
+     544             : 
+     545             : /* computeMaxDerivativesHeading() //{ */
+     546             : 
+     547             : // compute max velocity, acceleration and jerk
+     548           1 : bool Trajectory::computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max) const {
+     549             : 
+     550           1 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     551             : 
+     552           1 :   dimensions.push_back(3);  // 3 = heading
+     553             : 
+     554           1 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     555             : 
+     556           1 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj);
+     557           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj);
+     558           1 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj);
+     559             : 
+     560           1 :   *v_max = v_max_traj.value;
+     561           1 :   *a_max = a_max_traj.value;
+     562           1 :   *j_max = j_max_traj.value;
+     563             : 
+     564           2 :   return success;
+     565             : }
+     566             : 
+     567             : //}
+     568             : 
+     569             : /* scaleSegmentTimes() //{ */
+     570             : 
+     571           0 : bool Trajectory::scaleSegmentTimes(double scaling) {
+     572           0 :   if (scaling < 1.0e-6)
+     573             :     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           1 : 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           1 :   constexpr size_t kMaxCounter = 20;
+     604           1 :   constexpr double kTolerance  = 1e-3;
+     605             : 
+     606           1 :   bool within_range = false;
+     607             : 
+     608           1 :   for (size_t i = 0; i < kMaxCounter; i++) {
+     609             : 
+     610           2 :     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           1 :       double v_max_horizontal_actual, a_max_horizontal_actual, j_max_horizontal_actual;
+     616           1 :       computeMaxDerivativesHorizontal(&v_max_horizontal_actual, &a_max_horizontal_actual, &j_max_horizontal_actual, seg);
+     617             : 
+     618           1 :       double v_max_vertical_actual, a_max_vertical_actual, j_max_vertical_actual;
+     619           1 :       computeMaxDerivativesVertical(&v_max_vertical_actual, &a_max_vertical_actual, &j_max_vertical_actual, seg);
+     620             : 
+     621           1 :       double v_max_heading_actual, a_max_heading_actual, j_max_heading_actual;
+     622           1 :       computeMaxDerivativesHeading(&v_max_heading_actual, &a_max_heading_actual, &j_max_heading_actual, seg);
+     623             : 
+     624             :       // Reevaluate constraint/bound violation
+     625           1 :       double velocity_violation_horizontal     = v_max_horizontal_actual / v_max_horizontal;
+     626           1 :       double velocity_violation_vertical       = v_max_vertical_actual / v_max_vertical;
+     627           1 :       double acceleration_violation_horizontal = a_max_horizontal_actual / a_max_horizontal;
+     628           1 :       double acceleration_violation_vertical   = a_max_vertical_actual / a_max_vertical;
+     629           1 :       double jerk_violation_horizontal         = j_max_horizontal_actual / j_max_horizontal;
+     630           1 :       double jerk_violation_vertical           = j_max_vertical_actual / j_max_vertical;
+     631             : 
+     632           1 :       double velocity_violation_heading     = v_max_heading_actual / v_max_heading;
+     633           1 :       double acceleration_violation_heading = a_max_heading_actual / a_max_heading;
+     634           1 :       double jerk_violation_heading         = j_max_heading_actual / j_max_heading;
+     635             : 
+     636           1 :       double velocity_violation     = std::max(std::max(velocity_violation_horizontal, velocity_violation_vertical), velocity_violation_heading);
+     637           1 :       double acceleration_violation = std::max(std::max(acceleration_violation_horizontal, acceleration_violation_vertical), acceleration_violation_heading);
+     638           1 :       double jerk_violation         = std::max(std::max(jerk_violation_horizontal, jerk_violation_vertical), jerk_violation_heading);
+     639             : 
+     640           1 :       within_range = velocity_violation <= 1.0 + kTolerance && acceleration_violation <= 1.0 + kTolerance && jerk_violation <= 1.0 + kTolerance;
+     641             : 
+     642           2 :       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           1 :       double violation_scaling_inverse = 1.0 / violation_scaling;
+     646             : 
+     647             :       // Scale the segment times of each segment.
+     648           1 :       double new_max_time = 0.0;
+     649           1 :       double new_time     = segments_[seg].getTime() * violation_scaling;
+     650             : 
+     651           5 :       for (int d = 0; d < segments_[seg].D(); d++) {
+     652           4 :         (segments_[seg])[d].scalePolynomialInTime(violation_scaling_inverse);
+     653             :       }
+     654             : 
+     655           1 :       segments_[seg].setTime(new_time);
+     656           1 :       new_max_time += new_time;
+     657           1 :       max_time_ = new_max_time;
+     658             :     }
+     659             : 
+     660           1 :     double v_max_horizontal_actual, a_max_horizontal_actual, j_max_horizontal_actual;
+     661           1 :     computeMaxDerivativesHorizontal(&v_max_horizontal_actual, &a_max_horizontal_actual, &j_max_horizontal_actual);
+     662             : 
+     663           1 :     double v_max_vertical_actual, a_max_vertical_actual, j_max_vertical_actual;
+     664           1 :     computeMaxDerivativesVertical(&v_max_vertical_actual, &a_max_vertical_actual, &j_max_vertical_actual);
+     665             : 
+     666           1 :     double v_max_heading_actual, a_max_heading_actual, j_max_heading_actual;
+     667           1 :     computeMaxDerivativesHeading(&v_max_heading_actual, &a_max_heading_actual, &j_max_heading_actual);
+     668             : 
+     669             :     // Reevaluate constraint/bound violation
+     670           1 :     double velocity_violation_horizontal     = v_max_horizontal_actual / v_max_horizontal;
+     671           1 :     double velocity_violation_vertical       = v_max_vertical_actual / v_max_vertical;
+     672           1 :     double acceleration_violation_horizontal = a_max_horizontal_actual / a_max_horizontal;
+     673           1 :     double acceleration_violation_vertical   = a_max_vertical_actual / a_max_vertical;
+     674           1 :     double jerk_violation_horizontal         = j_max_horizontal_actual / j_max_horizontal;
+     675           1 :     double jerk_violation_vertical           = j_max_vertical_actual / j_max_vertical;
+     676             : 
+     677           1 :     double velocity_violation_heading     = v_max_heading_actual / v_max_heading;
+     678           1 :     double acceleration_violation_heading = a_max_heading_actual / a_max_heading;
+     679           1 :     double jerk_violation_heading         = j_max_heading_actual / j_max_heading;
+     680             : 
+     681           1 :     double velocity_violation     = std::max(std::max(velocity_violation_horizontal, velocity_violation_vertical), velocity_violation_heading);
+     682           1 :     double acceleration_violation = std::max(std::max(acceleration_violation_horizontal, acceleration_violation_vertical), acceleration_violation_heading);
+     683           1 :     double jerk_violation         = std::max(std::max(jerk_violation_horizontal, jerk_violation_vertical), jerk_violation_heading);
+     684             : 
+     685           1 :     within_range = velocity_violation <= 1.0 + kTolerance && acceleration_violation <= 1.0 + kTolerance && jerk_violation <= 1.0 + kTolerance;
+     686             : 
+     687           1 :     if (within_range) {
+     688             :       break;
+     689             :     }
+     690             :   }
+     691           1 :   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..288e92b4ae2d04242e6c225b8989814f72a4c440 GIT binary patch literal 2544 zcmVZz~W!dw#dwZ$^-B^4sfvaaRh>m464d_APzw~VteYejv|^Y`9oRiBObN# zB1HvIM^W7d3<>4o;@aiT)1m7G#XD<$Lh<$5UbOAu=lj>&8e6wU;IVi#!68dyq-z2A3Lf7g`f-5r zZz6D&hIQ`lIIg|M${chgOdaJfOz}#fca18rH#m3f4Nkb=v!98XF7Vi=$+%B58IyV| z1r!J=WmjP8xq7_5StNF@SO#ou4ZZu&x9wqI>=}?HhF!FHw;p;TO08`$3pQb%*OTdq zHy^|XFzEmjzM@}P^$;W0)DzWM8)FSpdv`PgNK+i!%0YNCNO^%sxFh5{r7R65yelgV zpHFwNEz7jYl#`N*3hQ9>J8wXwlzGypm66PbMPwbOOn#x zJTA;K0WJa27pMI1M3`^N*Gy_Z@CpidiafH5Q_t1;+OtSZT`&&K*LKQPOxYf&7&rIi zYh8*{`t8Vut25z*7R^kkYFN?0nh+L#9OH~fVwG~-qi!}caT0l8TIeTc=^JW857by@ zbyLzkOyhh$hX;1*1RkiE`GkjDl~d?^9Hdq}rYId!?~Y?38HPX@T0<^WCPQNDN`AXp z>US2Y1vQ>NDru=34|5g(pHo#}HMn zG*$GT_7r#7Ff0#2QptWdU%lQTYsTUqWKgE?%~exu;;fWXh?~)LUi3$W4fX!Ga;Y7= zOmN^l;qgeAi+L1VHhMn{d!aN$o%La1&jKnG*NTgi29uA&g^GsBM?RY=0dESV$DQlr zZ zeX8rw^Qd1}jL*T^hDu0a1*bEC0$FVs8!K8_%W(L&7=11`O z@3u+l+nwp-s+I0&g~TX{(MhmRka=i!@zJel6H8C26VY1pRHEjA`gu=`zg3`;V8bls z3APr>VZ1N|FYNico)t8x06eHNg*>BcP)qUrV>Lxs!-5hCZ08hqGa(RFZsH+?u!D%1 zcZG7{h6}Fc!VvJa$Cn+xwFjp5R|4Fz{J1&cTp4oPUi%$o9GLhVJ-K$IT3FIxR5cld8hSkKC{6^hz%{Tm8ZiZ=`eX)J8?mvJRm;kS^v+$jW3Bpo?4K5hZ? znNv6(H5-8*wO)AHae&;`J9`@}lSx4Dj%iC%hZ^qVZN># zKc)*}W%aZGqb>Fb_siod;7g#0c8p))CvS3L0JLQcHvp9l4?!M=6Y98CimNPS_O*1? zKDNZ@4HKlxDe9XY27P}##u`?{`7VaC)S1RN@mrOpbFQD3XRfO${=chSe-cPH`pQE} z)z+zrUF4BYIguB#M(_$u0{e&iahne12Z8IC8#GY-;?=!RQ#Ra()syF8+;bjomv`wKwxP!x z$>X*0;k8GgLDmMmqB}e6R9gk0_Y_uQcmk}58KfNKh)~f_2qRD$=;KA?^g^4Fb3K*r zI7)!CO=1P0HqVTqQ+H6R2Gmnb&(Fwr{WaZC6-RTeqHZ+=D?F(%WubO z*Oc8>Tcdr0)$-ArTgGxUvdOXiAOc4Ybnj(~$#lC&@l8`jaU69#jwui+o;0DG?#>Cl zk}$Id=mxV4JjEmDk2Ilc4KU>|0k-hCP&bHSZRP>sl+=N4=6B6w1`@lW&*g^g)64Ye zy3dc&p6L2$CQiFPy7qR#$Fn(MmGIf7aGTWFjkZFXa}-5)VQqFm)uW+)?jiJCJp{eL z?TxD_YOwO6h!S$H&TTPP>P0pOFU7yQb%A7gYx@PP)U+Hj|Gjq!< zV$NJ7;3(uH5x6XQ&MW|R6vt7= + + + + + + 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:2023-12-06 07:59:45Functions: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> >*)1
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> >*)1
+
+
+ + + +
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..c060cb7b9f --- /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:2023-12-06 07:59:45Functions: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> >*)1
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> >*)1
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..a95e698ae3 --- /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:2023-12-06 07:59:45Functions: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           1 : bool sampleTrajectoryInRange(const Trajectory& trajectory, double min_time, double max_time, double sampling_interval,
+      50             :                              eth_mav_msgs::EigenTrajectoryPointVector* states) {
+      51           1 :   CHECK_NOTNULL(states);
+      52           1 :   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           1 :   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           2 :   std::vector<Eigen::VectorXd> position, velocity, acceleration, jerk, snap, yaw, yaw_rate;
+      64             : 
+      65           1 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::POSITION, &position);
+      66           1 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::VELOCITY, &velocity);
+      67           1 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::ACCELERATION, &acceleration);
+      68           1 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::JERK, &jerk);
+      69           1 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::SNAP, &snap);
+      70             : 
+      71           1 :   size_t n_samples = position.size();
+      72             : 
+      73           1 :   states->resize(n_samples);
+      74          41 :   for (size_t i = 0; i < n_samples; ++i) {
+      75          40 :     eth_mav_msgs::EigenTrajectoryPoint& state = (*states)[i];
+      76             : 
+      77             :     /* state.degrees_of_freedom = eth_mav_msgs::MavActuation::DOF4; */
+      78          40 :     state.position_W         = position[i].head<3>();
+      79          40 :     state.velocity_W         = velocity[i].head<3>();
+      80          40 :     state.acceleration_W     = acceleration[i].head<3>();
+      81          40 :     state.jerk_W             = jerk[i].head<3>();
+      82          40 :     state.snap_W             = snap[i].head<3>();
+      83          40 :     state.time_from_start_ns = static_cast<int64_t>((min_time + sampling_interval * i) * kNumNanosecondsPerSecond);
+      84          40 :     if (trajectory.D() == 4) {
+      85          40 :       state.setFromYaw(position[i](3));
+      86          40 :       state.setFromYawRate(velocity[i](3));
+      87          40 :       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           1 :   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           1 : bool sampleWholeTrajectory(const Trajectory& trajectory, double sampling_interval, eth_mav_msgs::EigenTrajectoryPoint::Vector* states) {
+     120           1 :   const double min_time = trajectory.getMinTime();
+     121           1 :   const double max_time = trajectory.getMaxTime();
+     122             : 
+     123           1 :   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..34af35f7e0 --- /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:10626240.5 %
Date:2023-12-06 07:59:45Functions: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)1
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)1
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)1
eth_trajectory_generation::Vertex::makeStartOrEnd(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, int)2
eth_trajectory_generation::Vertex::addConstraint(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)14
eth_trajectory_generation::Vertex::getConstraint(int, Eigen::Matrix<double, -1, 1, 0, -1, 1>*) const14
+
+
+ + + +
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..2b7fb1d149 --- /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:10626240.5 %
Date:2023-12-06 07:59:45Functions: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)1
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)1
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)1
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&)14
eth_trajectory_generation::Vertex::makeStartOrEnd(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, int)2
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>*) const14
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..42ef2c8b41 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.html @@ -0,0 +1,670 @@ + + + + + + + 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:10626240.5 %
Date:2023-12-06 07:59:45Functions: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           0 :     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             :         break;
+      73             :       }
+      74             :     }
+      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          14 : void Vertex::addConstraint(int derivative_order, const Eigen::VectorXd& constraint) {
+     135          14 :   CHECK_EQ(constraint.rows(), static_cast<long>(D_));
+     136          14 :   constraints_[derivative_order] = constraint;
+     137          14 : }
+     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             :     return false;
+     151             :   }
+     152             : }
+     153             : 
+     154             : //}
+     155             : 
+     156             : /* makeStartOrEnd() //{ */
+     157             : 
+     158           2 : void Vertex::makeStartOrEnd(const Eigen::VectorXd& constraint, int up_to_derivative) {
+     159           2 :   addConstraint(derivative_order::POSITION, constraint);
+     160           6 :   for (int i = 1; i <= up_to_derivative; ++i) {
+     161           4 :     constraints_[i] = ConstraintValue::Zero(static_cast<int>(D_));
+     162             :   }
+     163           2 : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* getConstraint() //{ */
+     168             : 
+     169          14 : bool Vertex::getConstraint(int derivative_order, Eigen::VectorXd* value) const {
+     170          14 :   CHECK_NOTNULL(value);
+     171          14 :   typename Constraints::const_iterator it = constraints_.find(derivative_order);
+     172          14 :   if (it != constraints_.end()) {
+     173          11 :     *value = it->second;
+     174          11 :     return true;
+     175             :   } else
+     176             :     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             :     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             :       return false;
+     204             :   }
+     205             :   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             :   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           1 : 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           1 :   return estimateSegmentTimesEuclidean(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+     270           1 :                                        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           1 : 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           1 :   CHECK_GE(vertices.size(), 2);
+     306           1 :   std::vector<double> segment_times;
+     307           1 :   segment_times.reserve(vertices.size() - 1);
+     308             : 
+     309             :   // for each vertex in the path
+     310           2 :   for (size_t i = 0; i < vertices.size() - 1; ++i) {
+     311             : 
+     312           2 :     Eigen::VectorXd start4d, end4d;
+     313             : 
+     314           1 :     vertices[i].getConstraint(derivative_order::POSITION, &start4d);
+     315           1 :     vertices[i + 1].getConstraint(derivative_order::POSITION, &end4d);
+     316             : 
+     317           1 :     Eigen::Vector3d start     = start4d.head(3);
+     318           1 :     Eigen::Vector3d end       = end4d.head(3);
+     319           1 :     double          start_hdg = start4d(3);
+     320           1 :     double          end_hdg   = end4d(3);
+     321             : 
+     322           1 :     double acceleration_time_1 = 0;
+     323           1 :     double acceleration_time_2 = 0;
+     324             : 
+     325           1 :     double jerk_time_1 = 0;
+     326           1 :     double jerk_time_2 = 0;
+     327             : 
+     328           1 :     double acc_1_coeff = 0;
+     329           1 :     double acc_2_coeff = 0;
+     330             : 
+     331           1 :     double distance = (end - start).norm();
+     332             : 
+     333           1 :     double inclinator = atan2(end(2) - start(2), sqrt(pow(end(0) - start(0), 2) + pow(end(1) - start(1), 2)));
+     334             : 
+     335           1 :     double v_max, a_max, j_max;
+     336             : 
+     337           1 :     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           1 :       v_max = fabs(v_max_horizontal / cos(inclinator));
+     341             :     }
+     342             : 
+     343           1 :     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           1 :       a_max = fabs(a_max_horizontal / cos(inclinator));
+     347             :     }
+     348             : 
+     349           1 :     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           1 :       j_max = fabs(j_max_horizontal / cos(inclinator));
+     353             :     }
+     354             : 
+     355           1 :     if (i >= 1) {
+     356             : 
+     357           0 :       Eigen::VectorXd pre4d;
+     358             : 
+     359           0 :       vertices[i - 1].getConstraint(derivative_order::POSITION, &pre4d);
+     360             : 
+     361           0 :       Eigen::Vector3d pre = pre4d.head(3);
+     362             : 
+     363           0 :       Eigen::Vector3d vec1 = start - pre;
+     364           0 :       Eigen::Vector3d vec2 = end - start;
+     365             : 
+     366           0 :       vec1.normalize();
+     367           0 :       vec2.normalize();
+     368             : 
+     369           0 :       double scalar = vec1.dot(vec2) < 0 ? 0.0 : vec1.dot(vec2);
+     370             : 
+     371           0 :       acc_1_coeff = (1 - scalar);
+     372             : 
+     373           0 :       acceleration_time_1 = acc_1_coeff * ((v_max / a_max) + (a_max / j_max));
+     374             : 
+     375           0 :       jerk_time_1 = acc_1_coeff * (2 * (a_max / j_max));
+     376             :     }
+     377             : 
+     378             :     // the first vertex
+     379           1 :     if (i == 0) {
+     380           1 :       acc_1_coeff         = 1.0;
+     381           1 :       acceleration_time_1 = (v_max / a_max) + (a_max / j_max);
+     382           1 :       jerk_time_1         = (2 * (a_max / j_max));
+     383             :     }
+     384             : 
+     385             :     // last vertex
+     386           1 :     if (i == vertices.size() - 2) {
+     387           1 :       acc_2_coeff         = 1.0;
+     388           1 :       acceleration_time_2 = (v_max / a_max) + (a_max / j_max);
+     389           1 :       jerk_time_2         = (2 * (a_max / j_max));
+     390             :     }
+     391             : 
+     392             :     // a vertex
+     393           1 :     if (i < vertices.size() - 2) {
+     394             : 
+     395           0 :       Eigen::VectorXd post4d;
+     396             : 
+     397           0 :       vertices[i + 2].getConstraint(derivative_order::POSITION, &post4d);
+     398             : 
+     399           0 :       Eigen::Vector3d post = post4d.head(3);
+     400             : 
+     401           0 :       Eigen::Vector3d vec1 = end - start;
+     402           0 :       Eigen::Vector3d vec2 = post - end;
+     403             : 
+     404           0 :       vec1.normalize();
+     405           0 :       vec2.normalize();
+     406             : 
+     407           0 :       double scalar = vec1.dot(vec2) < 0 ? 0.0 : vec1.dot(vec2);
+     408             : 
+     409           0 :       acc_2_coeff = (1 - scalar);
+     410             : 
+     411           0 :       acceleration_time_2 = acc_2_coeff * ((v_max / a_max) + (a_max / j_max));
+     412             : 
+     413           0 :       jerk_time_2 = acc_2_coeff * (2 * (a_max / j_max));
+     414             :     }
+     415             : 
+     416           1 :     if (acceleration_time_1 > sqrt(2 * distance / a_max)) {
+     417           0 :       acceleration_time_1 = sqrt(2 * distance / a_max);
+     418             :     }
+     419             : 
+     420           1 :     if (jerk_time_1 > sqrt(2 * v_max / j_max)) {
+     421           0 :       jerk_time_1 = sqrt(2 * v_max / j_max);
+     422             :     }
+     423             : 
+     424           1 :     if (acceleration_time_2 > sqrt(2 * distance / a_max)) {
+     425           0 :       acceleration_time_2 = sqrt(2 * distance / a_max);
+     426             :     }
+     427             : 
+     428           1 :     if (jerk_time_2 > sqrt(2 * v_max / j_max)) {
+     429           0 :       jerk_time_2 = sqrt(2 * v_max / j_max);
+     430             :     }
+     431             : 
+     432           1 :     double max_velocity_time;
+     433             : 
+     434           1 :     if (((distance - (2 * (v_max * v_max) / a_max)) / v_max) < 0) {
+     435           0 :       max_velocity_time = ((distance) / v_max);
+     436             :     } else {
+     437             :       max_velocity_time = ((distance - (2 * (v_max * v_max) / a_max)) / v_max);
+     438             :     }
+     439             : 
+     440             :     /* double t = max_velocity_time + acceleration_time_1 + acceleration_time_2 + jerk_time_1 + jerk_time_2; */
+     441           1 :     double t = max_velocity_time + acceleration_time_1 + acceleration_time_2;
+     442             : 
+     443             :     /* 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); */
+     444             :     /* printf("segment %d time %.2f, distance %.2f, %.2f, %.2f, %.2f, vmax: %.2f, amax: %.2f, jmax: %.2f\n", i, t, distance, max_velocity_time, */
+     445             :     /*        acceleration_time_1, acceleration_time_2, v_max, a_max, j_max); */
+     446             : 
+     447           1 :     if (t < 0.01) {
+     448           0 :       t = 0.01;
+     449             :     }
+     450             : 
+     451             :     // | ------------- check the heading rotation time ------------ |
+     452             : 
+     453           1 :     double angular_distance = fabs(mrs_lib::geometry::radians::dist(start_hdg, end_hdg));
+     454             : 
+     455           1 :     double hdg_velocity_time     = 0;
+     456           1 :     double hdg_acceleration_time = 0;
+     457             : 
+     458           1 :     if (heading_speed_max < std::numeric_limits<float>::max() && heading_acc_max < std::numeric_limits<float>::max()) {
+     459             : 
+     460           1 :       if (((angular_distance - (2 * (heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max) < 0) {
+     461           0 :         hdg_velocity_time = ((angular_distance) / heading_speed_max);
+     462             :       } else {
+     463             :         hdg_velocity_time = ((angular_distance - (2 * (heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max);
+     464             :       }
+     465             : 
+     466           1 :       if (angular_distance > M_PI / 4) {
+     467           1 :         hdg_acceleration_time = 2 * (heading_speed_max / heading_acc_max);
+     468             :       }
+     469             :     }
+     470             : 
+     471             :     // what will take longer? to fix the lateral or the heading
+     472           1 :     double heading_fix_time = 1.5 * (hdg_velocity_time + hdg_acceleration_time);
+     473             : 
+     474           1 :     if (heading_fix_time > t) {
+     475           0 :       t = heading_fix_time;
+     476             :     }
+     477             : 
+     478           1 :     segment_times.push_back(t);
+     479             :   }
+     480           1 :   return segment_times;
+     481             : }
+     482             : 
+     483             : //}
+     484             : 
+     485             : /* estimateSegmentTimesEuclidean() //{ */
+     486             : 
+     487           1 : std::vector<double> estimateSegmentTimesEuclidean(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     488             :                                                   const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     489             :                                                   const double j_max_vertical, const double heading_speed_max, const double heading_acc_max) {
+     490             : 
+     491           1 :   double v_max = std::min(v_max_horizontal, v_max_vertical);
+     492             : 
+     493           1 :   CHECK_GE(vertices.size(), 2);
+     494           1 :   std::vector<double> segment_times;
+     495           1 :   segment_times.reserve(vertices.size() - 1);
+     496             : 
+     497             :   // for each vertex in the path
+     498           2 :   for (size_t i = 0; i < vertices.size() - 1; ++i) {
+     499             : 
+     500           2 :     Eigen::VectorXd start4d, end4d;
+     501             : 
+     502           1 :     vertices[i].getConstraint(derivative_order::POSITION, &start4d);
+     503           1 :     vertices[i + 1].getConstraint(derivative_order::POSITION, &end4d);
+     504             : 
+     505           1 :     Eigen::Vector3d start = start4d.head(3);
+     506           1 :     Eigen::Vector3d end   = end4d.head(3);
+     507             : 
+     508           1 :     double inclinator = atan2(end(2) - start(2), sqrt(pow(end(0) - start(0), 2) + pow(end(1) - start(1), 2)));
+     509             : 
+     510           1 :     double v_max;
+     511             : 
+     512           1 :     if (inclinator > atan2(v_max_vertical, v_max_horizontal) || inclinator < -atan2(v_max_vertical, v_max_horizontal)) {
+     513           0 :       v_max = fabs(v_max_vertical / sin(inclinator));
+     514             :     } else {
+     515           1 :       v_max = fabs(v_max_horizontal / cos(inclinator));
+     516             :     }
+     517             : 
+     518           1 :     double start_hdg = start4d(3);
+     519           1 :     double end_hdg   = end4d(3);
+     520             : 
+     521           1 :     double distance = (end - start).norm();
+     522             : 
+     523           1 :     double t = distance / v_max;
+     524             : 
+     525           1 :     if (t < 0.01) {
+     526           0 :       t = 0.01;
+     527             :     }
+     528             : 
+     529             :     // | ------------- check the heading rotation time ------------ |
+     530             : 
+     531             : 
+     532           1 :     double angular_distance = fabs(mrs_lib::geometry::radians::dist(start_hdg, end_hdg));
+     533             : 
+     534           1 :     double hdg_velocity_time     = 0;
+     535           1 :     double hdg_acceleration_time = 0;
+     536             : 
+     537           1 :     if (heading_speed_max < std::numeric_limits<float>::max() && heading_acc_max < std::numeric_limits<float>::max()) {
+     538             : 
+     539           1 :       if (((angular_distance - ((heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max) < 0) {
+     540           0 :         hdg_velocity_time = ((angular_distance) / heading_speed_max);
+     541             :       } else {
+     542             :         hdg_velocity_time = ((angular_distance - ((heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max);
+     543             :       }
+     544             : 
+     545           1 :       if (angular_distance > M_PI / 4) {
+     546           1 :         hdg_acceleration_time = 2 * (heading_speed_max / heading_acc_max);
+     547             :       }
+     548             :     }
+     549             : 
+     550             :     // what will take longer? to fix the lateral or the heading
+     551           1 :     double heading_fix_time = 1.5 * (hdg_velocity_time + hdg_acceleration_time);
+     552             : 
+     553           1 :     if (heading_fix_time > t) {
+     554           1 :       t = heading_fix_time;
+     555             :     }
+     556             : 
+     557           1 :     segment_times.push_back(t);
+     558             :   }
+     559             : 
+     560           1 :   return segment_times;
+     561             : }
+     562             : 
+     563             : //}
+     564             : 
+     565             : /* computeTimeVelocityRamp() //{ */
+     566             : 
+     567           0 : double computeTimeVelocityRamp(const Eigen::VectorXd& start, const Eigen::VectorXd& goal, double v_max, double a_max) {
+     568             : 
+     569           0 :   const double distance = (start - goal).norm();
+     570             :   // Time to accelerate or decelerate to or from maximum velocity:
+     571           0 :   const double acc_time = v_max / a_max;
+     572             :   // Distance covered during complete acceleration or decelerate:
+     573           0 :   const double acc_distance = 0.5 * v_max * acc_time;
+     574             :   // Compute total segment time:
+     575           0 :   if (distance < 2.0 * acc_distance) {
+     576             :     // Case 1: Distance too small to accelerate to maximum velocity.
+     577           0 :     return 2.0 * std::sqrt(distance / a_max);
+     578             :   } else {
+     579             :     // Case 2: Distance long enough to accelerate to maximum velocity.
+     580           0 :     return 2.0 * acc_time + (distance - 2.0 * acc_distance) / v_max;
+     581             :   }
+     582             : }
+     583             : 
+     584             : //}
+     585             : 
+     586             : }  // 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..dd27b839f0 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.overview.html @@ -0,0 +1,167 @@ + + + + + + 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 + + +
+ 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..04d0fb01135fc5cbc45191645964f3a52d8da680 GIT binary patch literal 2255 zcmV;=2r&1FP)w0T2}$?_nU<-u%=5CB zdH*^7spCe$z5Bl`ED*p9E(7{!^A1)E&|ee-tl&5v1>lg>Y5^i}Y>rrZ&18~N7cjLL*JWIGmAV*}QG+6Z{>a-_#4Ei^yM;16hG387I z_EWU3{p`-60bvbKv1sd}90aD9#8ghPyHGv4>SbXf(7rsM#+3YwE1bwI0mt;1X_4dc z4n5K;tzW7#i_qLyt>=;%Bi?t6o#|2L+Q2xY*XCT*id`FyRt0Ch9^B&XPJU_TEI>g z2SUnqy-Xmj&@c$y*l0 z;Zf8hYv%D!K1+|s^4293pU?G4%1SQqS|1Qq!=y4dX$?cGvrQ{*SrgMQLr0f(8vHi*#KyTOsr)j#vOOEd;5H*>sunH>oORP;r z{$F#CTmAYfE6e>lI_wIc@VIbSiq8C6AI`jLO{0V^ym)t4q@o$pk?i#7i~ye%DSjd} zhekTWG;PAFraUmUi^ZO!4cZnvl>>widFn6K$<~ zQ{yx}iU~Wv7{KuuFp;)zcdu`zyU1I?}!8lO*`g&+ zB?!e&i{aNqaUq5mQCwIVE~B_mPa4J5(EjCQgG8HG%OjIKutw;S;_F05jc+ zFxu6ok5mBa_*g1l&sH7N1<+oCf=*CmiY9tR;L7!|0DVK4U5;Q`HQ*ty*x(-ME}@y- zmsB+ZwWr9ba!{D4o4uLe2aZ=QQX2(+P}N#TtK{2ylwwH6tvIfwcY2&6WS=V0Y=GiQ zm;uJZg~n%o(S_pp5+bm3g?*sJH$5-&>lkP%8yo^K#HS-phKpwj1d0KclfEIE zg(Ol72I|4!F6!y`si$iLn^TXD(yUU|T?{^oUZJ*xJnDtDUcsKC7-G24Eu5!V3-$?6 zdUNpGD|~we-d)S!X%n4A1vu9NC?*LL=(sMH4G#L5x7SYU{HM9n~?oH%ks{_~8 z_YBzQm|hJB_k<&F)Tq%U&L!<*mvY0Ba}jraJftPVd@)sATRh>{tSoVaK0@)4uJ>wk zL#o1;$!Vy_FVQIaLWOVb(mf$u;EkW0TpTJAsn=B;vpYQ!1#4D40PLE6TrbOnv$#2K zqr&r$Fw&ybsVlI;3|bFWqa0W;q%_CPclEBM4P{|^Ut%F@tiPr4kvlZr-VsdVQ5m7g zw`^u1Cjs^-GapVybDa0{2u+yg3z;0>fGebwzPaHXNQM4QxnVIgoacrs&HYcL0o%ZS zihnaTgqu{VZ_!2I!vil-_$}GyIGw+Q$3HmxIAJ|cZfGBz=Q~YF_3#}=0lcOAAZ;jD-8)U`uR7}cx6Nm+c)f8G-))2q*>F3wn;Z9X$K`Nl zwR!UI?!xJU_+`TjDr8G_O3_MtN#K=byAeFdWRrncaWZEhTfPECU$&~AqJ@X5p}c(b z#aEU+b9PHLK*Yw^s$)u%U%VbY-9(X2pPU#Cg8*+7v}$XKjshE@>W+F3;3vGhr)E5R dJr=H#{{X*<=1#mlIJ^J=002ovPDHLkV1gbiNG$*W literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/index-detail-sort-f.html b/mrs_uav_trajectory_generation/src/index-detail-sort-f.html new file mode 100644 index 0000000000..61bb920103 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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 +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
<unnamed>43.4 %449 / 103478.3 %18 / 23
+
+
+ + + + +
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..ffa8991ea5 --- /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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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 +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
<unnamed>43.4 %449 / 103478.3 %18 / 23
+
+
+ + + + +
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..2fd4c0d77c --- /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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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 +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
<unnamed>43.4 %449 / 103478.3 %18 / 23
+
+
+ + + + +
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..6c020fddd2 --- /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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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 +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
+
+
+ + + + +
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..6380e49fbe --- /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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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 +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
+
+
+ + + + +
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..769b84ac0c --- /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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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 +
43.4%43.4%
+ 		43.4 %449 / 103478.3 %18 / 23
+
+
+ + + + +
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..59facaf5b6 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + 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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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> const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)1
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&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, ros::Time const&, double const&)1
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&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> > const&, bool, bool)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()4
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)42
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()42
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&)78
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)3831
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackControlManagerDiag(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)4897
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackTrackerCmd(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)31927
+
+
+ + + +
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..7d3a1888ca --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + 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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)42
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)1
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&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)1
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&)78
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackTrackerCmd(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)31927
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)3831
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, ros::Time const&, double const&)1
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&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackControlManagerDiag(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)4897
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()42
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> > const&, bool, bool)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()4
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()1
+
+
+ + + +
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..ca80903421 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html @@ -0,0 +1,2416 @@ + + + + + + + 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:449103443.4 %
Date:2023-12-06 07:59:45Functions:182378.3 %
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             : 
+      23             : #include <eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h>
+      24             : #include <eth_trajectory_generation/trajectory.h>
+      25             : #include <eth_trajectory_generation/trajectory_sampling.h>
+      26             : 
+      27             : #include <mrs_lib/param_loader.h>
+      28             : #include <mrs_lib/geometry/cyclic.h>
+      29             : #include <mrs_lib/geometry/misc.h>
+      30             : #include <mrs_lib/mutex.h>
+      31             : #include <mrs_lib/batch_visualizer.h>
+      32             : #include <mrs_lib/transformer.h>
+      33             : #include <mrs_lib/utils.h>
+      34             : #include <mrs_lib/scope_timer.h>
+      35             : #include <mrs_lib/attitude_converter.h>
+      36             : #include <mrs_lib/publisher_handler.h>
+      37             : 
+      38             : #include <dynamic_reconfigure/server.h>
+      39             : #include <mrs_uav_trajectory_generation/drsConfig.h>
+      40             : 
+      41             : #include <future>
+      42             : 
+      43             : //}
+      44             : 
+      45             : /* using //{ */
+      46             : 
+      47             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      48             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      49             : using mat3_t = Eigen::Matrix3Xd;
+      50             : 
+      51             : using radians  = mrs_lib::geometry::radians;
+      52             : using sradians = mrs_lib::geometry::sradians;
+      53             : 
+      54             : //}
+      55             : 
+      56             : /* defines //{ */
+      57             : 
+      58             : #define FUTURIZATION_EXEC_TIME_FACTOR 0.5        // [0, 1]
+      59             : #define FUTURIZATION_FIRST_WAYPOINT_FACTOR 0.50  // [0, 1]
+      60             : #define OVERTIME_SAFETY_FACTOR 0.95              // [0, 1]
+      61             : #define OVERTIME_SAFETY_OFFSET 0.01              // [s]
+      62             : #define NLOPT_EXEC_TIME_FACTOR 0.95              // [0, 1]
+      63             : 
+      64             : typedef struct
+      65             : {
+      66             :   Eigen::Vector4d coords;
+      67             :   bool            stop_at;
+      68           9 : } Waypoint_t;
+      69             : 
+      70             : //}
+      71             : 
+      72             : namespace mrs_uav_trajectory_generation
+      73             : {
+      74             : 
+      75             : /* class MrsTrajectoryGeneration //{ */
+      76             : class MrsTrajectoryGeneration : public nodelet::Nodelet {
+      77             : 
+      78             : public:
+      79             :   virtual void onInit();
+      80             : 
+      81             : private:
+      82             :   ros::NodeHandle nh_;
+      83             : 
+      84             :   bool is_initialized_ = false;
+      85             : 
+      86             :   // | ----------------------- parameters ----------------------- |
+      87             : 
+      88             :   double _sampling_dt_;
+      89             : 
+      90             :   double _max_trajectory_len_factor_;
+      91             :   double _min_trajectory_len_factor_;
+      92             : 
+      93             :   int _n_attempts_;
+      94             : 
+      95             :   double _min_waypoint_distance_;
+      96             : 
+      97             :   bool   _fallback_sampling_enabled_;
+      98             :   double _fallback_sampling_speed_factor_;
+      99             :   double _fallback_sampling_accel_factor_;
+     100             :   double _fallback_sampling_stopping_time_;
+     101             :   bool   _fallback_sampling_first_waypoint_additional_stop_;
+     102             : 
+     103             :   std::string _uav_name_;
+     104             : 
+     105             :   bool   _trajectory_max_segment_deviation_enabled_;
+     106             :   double _trajectory_max_segment_deviation_;
+     107             :   int    _trajectory_max_segment_deviation_max_iterations_;
+     108             : 
+     109             :   bool   _path_straightener_enabled_;
+     110             :   double _path_straightener_max_deviation_;
+     111             :   double _path_straightener_max_hdg_deviation_;
+     112             : 
+     113             :   // | -------- variable parameters (come with the path) -------- |
+     114             : 
+     115             :   std::string frame_id_;
+     116             :   bool        fly_now_                              = false;
+     117             :   bool        use_heading_                          = false;
+     118             :   bool        stop_at_waypoints_                    = false;
+     119             :   bool        override_constraints_                 = false;
+     120             :   bool        loop_                                 = false;
+     121             :   double      override_max_velocity_horizontal_     = 0.0;
+     122             :   double      override_max_velocity_vertical_       = 0.0;
+     123             :   double      override_max_acceleration_horizontal_ = 0.0;
+     124             :   double      override_max_acceleration_vertical_   = 0.0;
+     125             :   double      override_max_jerk_horizontal_         = 0.0;
+     126             :   double      override_max_jerk_vertical_           = 0.0;
+     127             : 
+     128             :   // | -------------- variable parameters (deduced) ------------- |
+     129             : 
+     130             :   double     max_execution_time_ = 0;
+     131             :   std::mutex mutex_max_execution_time_;
+     132             : 
+     133             :   bool max_deviation_first_segment_;
+     134             : 
+     135             :   // | -------------------- the transformer  -------------------- |
+     136             : 
+     137             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     138             : 
+     139             :   // | ------------------- scope timer logger ------------------- |
+     140             : 
+     141             :   bool                                       scope_timer_enabled_ = false;
+     142             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     143             : 
+     144             :   // service client for input
+     145             :   bool               callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res);
+     146             :   ros::ServiceServer service_server_path_;
+     147             : 
+     148             :   // service client for returning result to the user
+     149             :   bool               callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res);
+     150             :   ros::ServiceServer service_server_get_path_;
+     151             : 
+     152             :   // subscriber for input
+     153             :   void            callbackPath(const mrs_msgs::PathConstPtr& msg);
+     154             :   ros::Subscriber subscriber_path_;
+     155             : 
+     156             :   void                          callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg);
+     157             :   ros::Subscriber               subscriber_constraints_;
+     158             :   bool                          got_constraints_ = false;
+     159             :   mrs_msgs::DynamicsConstraints constraints_;
+     160             :   std::mutex                    mutex_constraints_;
+     161             : 
+     162             :   void                     callbackTrackerCmd(const mrs_msgs::TrackerCommandConstPtr& msg);
+     163             :   ros::Subscriber          subscriber_tracker_cmd_;
+     164             :   bool                     got_tracker_cmd_ = false;
+     165             :   mrs_msgs::TrackerCommand tracker_cmd_;
+     166             :   std::mutex               mutex_tracker_cmd_;
+     167             : 
+     168             :   void                                callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg);
+     169             :   ros::Subscriber                     subscriber_control_manager_diag_;
+     170             :   bool                                got_control_manager_diag_ = false;
+     171             :   mrs_msgs::ControlManagerDiagnostics control_manager_diag_;
+     172             :   std::mutex                          mutex_control_manager_diag_;
+     173             : 
+     174             :   // service client for publishing trajectory out
+     175             :   ros::ServiceClient service_client_trajectory_reference_;
+     176             : 
+     177             :   // solve the whole problem
+     178             :   std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> optimize(const std::vector<Waypoint_t>& waypoints_in, const std_msgs::Header& waypoints_stamp,
+     179             :                                                                         const mrs_msgs::TrackerCommand&         initial_condition,
+     180             :                                                                         const mrs_msgs::MpcPredictionFullState& current_prediction, bool fallback_sampling,
+     181             :                                                                         const bool relax_heading);
+     182             : 
+     183             :   // batch vizualizer
+     184             :   mrs_lib::BatchVisualizer bw_original_;
+     185             :   mrs_lib::BatchVisualizer bw_final_;
+     186             : 
+     187             :   // transforming TrackerCommand
+     188             :   std::optional<mrs_msgs::Path> transformPath(const mrs_msgs::Path& path, const std::string& target_frame);
+     189             : 
+     190             :   // | ------------------ trajectory validation ----------------- |
+     191             : 
+     192             :   /**
+     193             :    * @brief validates samples of a trajectory agains a path of waypoints
+     194             :    *
+     195             :    * @param trajectory
+     196             :    * @param segments
+     197             :    *
+     198             :    * @return <success, traj_fail_idx, path_fail_segment>
+     199             :    */
+     200             :   std::tuple<bool, int, std::vector<bool>, double> validateTrajectorySpatial(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+     201             :                                                                              const std::vector<Waypoint_t>&                    waypoints);
+     202             : 
+     203             :   std::vector<int> getTrajectorySegmentCenterIdxs(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector<Waypoint_t>& waypoints);
+     204             : 
+     205             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectory(const std::vector<Waypoint_t>&  waypoints,
+     206             :                                                                            const mrs_msgs::TrackerCommand& initial_state, const double& sampling_dt,
+     207             :                                                                            const bool& relax_heading);
+     208             : 
+     209             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector>              findTrajectoryFallback(const std::vector<Waypoint_t>&  waypoints,
+     210             :                                                                                                 const mrs_msgs::TrackerCommand& initial_state, const double& sampling_dt,
+     211             :                                                                                                 const bool& relax_heading);
+     212             :   std::future<std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector>> future_trajectory_result_;
+     213             :   std::atomic<bool>                                                      running_async_planning_ = false;
+     214             : 
+     215             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectoryAsync(const std::vector<Waypoint_t>&  waypoints,
+     216             :                                                                                 const mrs_msgs::TrackerCommand& initial_state, const double& sampling_dt,
+     217             :                                                                                 const bool& relax_heading);
+     218             : 
+     219             :   std::vector<Waypoint_t> preprocessPath(const std::vector<Waypoint_t>& waypoints_in);
+     220             : 
+     221             :   mrs_msgs::TrajectoryReference getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const ros::Time& stamp,
+     222             :                                                        const double& sampling_dt);
+     223             : 
+     224             :   Waypoint_t interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff);
+     225             : 
+     226             :   bool checkNaN(const Waypoint_t& a);
+     227             : 
+     228             :   double distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2);
+     229             : 
+     230             :   bool trajectorySrv(const mrs_msgs::TrajectoryReference& msg);
+     231             : 
+     232             :   // | --------------- dynamic reconfigure server --------------- |
+     233             : 
+     234             :   boost::recursive_mutex                           mutex_drs_;
+     235             :   typedef mrs_uav_trajectory_generation::drsConfig DrsParams_t;
+     236             :   typedef dynamic_reconfigure::Server<DrsParams_t> Drs_t;
+     237             :   boost::shared_ptr<Drs_t>                         drs_;
+     238             :   void                                             callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, uint32_t level);
+     239             :   DrsParams_t                                      params_;
+     240             :   std::mutex                                       mutex_params_;
+     241             : 
+     242             :   // | ------------ Republisher for the desired path ------------ |
+     243             : 
+     244             :   mrs_lib::PublisherHandler<mrs_msgs::Path> ph_original_path_;
+     245             : 
+     246             :   // | ------------- measuring the time of execution ------------ |
+     247             : 
+     248             :   ros::Time  start_time_total_;
+     249             :   std::mutex mutex_start_time_total_;
+     250             :   bool       overtime(void);
+     251             :   double     timeLeft(void);
+     252             : };
+     253             : 
+     254             : //}
+     255             : 
+     256             : /* onInit() //{ */
+     257             : 
+     258          42 : void MrsTrajectoryGeneration::onInit() {
+     259             : 
+     260             :   /* obtain node handle */
+     261          42 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     262             : 
+     263             :   /* waits for the ROS to publish clock */
+     264          42 :   ros::Time::waitForValid();
+     265             : 
+     266             :   // | ----------------------- publishers ----------------------- |
+     267             : 
+     268         126 :   ph_original_path_ = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "original_path_out", 1);
+     269             : 
+     270             :   // | ----------------------- subscribers ---------------------- |
+     271             : 
+     272          42 :   subscriber_constraints_ = nh_.subscribe("constraints_in", 1, &MrsTrajectoryGeneration::callbackConstraints, this, ros::TransportHints().tcpNoDelay());
+     273          42 :   subscriber_tracker_cmd_ = nh_.subscribe("tracker_cmd_in", 1, &MrsTrajectoryGeneration::callbackTrackerCmd, this, ros::TransportHints().tcpNoDelay());
+     274          42 :   subscriber_control_manager_diag_ =
+     275          84 :       nh_.subscribe("control_manager_diag_in", 1, &MrsTrajectoryGeneration::callbackControlManagerDiag, this, ros::TransportHints().tcpNoDelay());
+     276          42 :   subscriber_path_ = nh_.subscribe("path_in", 1, &MrsTrajectoryGeneration::callbackPath, this, ros::TransportHints().tcpNoDelay());
+     277             : 
+     278             :   // | --------------------- service servers -------------------- |
+     279             : 
+     280          42 :   service_server_path_ = nh_.advertiseService("path_in", &MrsTrajectoryGeneration::callbackPathSrv, this);
+     281             : 
+     282          42 :   service_server_get_path_ = nh_.advertiseService("get_path_in", &MrsTrajectoryGeneration::callbackGetPathSrv, this);
+     283             : 
+     284          84 :   service_client_trajectory_reference_ = nh_.serviceClient<mrs_msgs::TrajectoryReferenceSrv>("trajectory_reference_out");
+     285             : 
+     286             :   // | ----------------------- parameters ----------------------- |
+     287             : 
+     288          42 :   mrs_lib::ParamLoader param_loader(nh_, "MrsTrajectoryGeneration");
+     289             : 
+     290          84 :   std::string custom_config_path;
+     291          42 :   std::string platform_config_path;
+     292             : 
+     293          42 :   param_loader.loadParam("custom_config", custom_config_path);
+     294          42 :   param_loader.loadParam("platform_config", platform_config_path);
+     295             : 
+     296          42 :   if (custom_config_path != "") {
+     297          42 :     param_loader.addYamlFile(custom_config_path);
+     298             :   }
+     299             : 
+     300          42 :   if (platform_config_path != "") {
+     301          42 :     param_loader.addYamlFile(platform_config_path);
+     302             :   }
+     303             : 
+     304          42 :   param_loader.addYamlFileFromParam("private_config");
+     305          42 :   param_loader.addYamlFileFromParam("public_config");
+     306             : 
+     307          84 :   const std::string yaml_prefix = "mrs_uav_trajectory_generation/";
+     308             : 
+     309          42 :   param_loader.loadParam("uav_name", _uav_name_);
+     310             : 
+     311          84 :   param_loader.loadParam(yaml_prefix + "sampling_dt", _sampling_dt_);
+     312             : 
+     313          84 :   param_loader.loadParam(yaml_prefix + "enforce_fallback_solver", params_.enforce_fallback_solver);
+     314             : 
+     315          84 :   param_loader.loadParam(yaml_prefix + "max_trajectory_len_factor", _max_trajectory_len_factor_);
+     316          84 :   param_loader.loadParam(yaml_prefix + "min_trajectory_len_factor", _min_trajectory_len_factor_);
+     317             : 
+     318          84 :   param_loader.loadParam(yaml_prefix + "n_attempts", _n_attempts_);
+     319          84 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/enabled", _fallback_sampling_enabled_);
+     320          84 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/speed_factor", _fallback_sampling_speed_factor_);
+     321          84 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/accel_factor", _fallback_sampling_accel_factor_);
+     322          84 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/stopping_time", _fallback_sampling_stopping_time_);
+     323          84 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/first_waypoint_additional_stop", _fallback_sampling_first_waypoint_additional_stop_);
+     324             : 
+     325          84 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/enabled", _trajectory_max_segment_deviation_enabled_);
+     326          84 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_deviation", _trajectory_max_segment_deviation_);
+     327          84 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_iterations", _trajectory_max_segment_deviation_max_iterations_);
+     328             : 
+     329          84 :   param_loader.loadParam(yaml_prefix + "path_straightener/enabled", _path_straightener_enabled_);
+     330          84 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_deviation", _path_straightener_max_deviation_);
+     331          84 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_hdg_deviation", _path_straightener_max_hdg_deviation_);
+     332             : 
+     333          84 :   param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_);
+     334             : 
+     335             :   // | --------------------- tf transformer --------------------- |
+     336             : 
+     337          42 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "TrajectoryGeneration");
+     338          42 :   transformer_->setDefaultPrefix(_uav_name_);
+     339          42 :   transformer_->retryLookupNewest(true);
+     340             : 
+     341             :   // | ------------------- scope timer logger ------------------- |
+     342             : 
+     343          84 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     344         126 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     345          42 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     346             : 
+     347             :   // | --------------------- service clients -------------------- |
+     348             : 
+     349          84 :   param_loader.loadParam(yaml_prefix + "time_penalty", params_.time_penalty);
+     350          84 :   param_loader.loadParam(yaml_prefix + "soft_constraints_enabled", params_.soft_constraints_enabled);
+     351          84 :   param_loader.loadParam(yaml_prefix + "soft_constraints_weight", params_.soft_constraints_weight);
+     352          84 :   param_loader.loadParam(yaml_prefix + "time_allocation", params_.time_allocation);
+     353          84 :   param_loader.loadParam(yaml_prefix + "equality_constraint_tolerance", params_.equality_constraint_tolerance);
+     354          84 :   param_loader.loadParam(yaml_prefix + "inequality_constraint_tolerance", params_.inequality_constraint_tolerance);
+     355          84 :   param_loader.loadParam(yaml_prefix + "max_iterations", params_.max_iterations);
+     356          84 :   param_loader.loadParam(yaml_prefix + "derivative_to_optimize", params_.derivative_to_optimize);
+     357             : 
+     358          84 :   param_loader.loadParam(yaml_prefix + "max_time", params_.max_time);
+     359          42 :   max_execution_time_ = params_.max_time;
+     360             : 
+     361          42 :   if (!param_loader.loadedSuccessfully()) {
+     362           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: could not load all parameters!");
+     363           0 :     ros::shutdown();
+     364             :   }
+     365             : 
+     366             :   // | -------------------- batch visualizer -------------------- |
+     367             : 
+     368          84 :   bw_original_ = mrs_lib::BatchVisualizer(nh_, "markers/original", "");
+     369             : 
+     370          42 :   bw_original_.clearBuffers();
+     371          42 :   bw_original_.clearVisuals();
+     372             : 
+     373          42 :   bw_final_ = mrs_lib::BatchVisualizer(nh_, "markers/final", "");
+     374             : 
+     375          42 :   bw_final_.clearBuffers();
+     376          42 :   bw_final_.clearVisuals();
+     377             : 
+     378             :   // | --------------- dynamic reconfigure server --------------- |
+     379             : 
+     380          42 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     381          42 :   drs_->updateConfig(params_);
+     382          84 :   Drs_t::CallbackType f = boost::bind(&MrsTrajectoryGeneration::callbackDrs, this, _1, _2);
+     383          42 :   drs_->setCallback(f);
+     384             : 
+     385             :   // | --------------------- finish the init -------------------- |
+     386             : 
+     387          84 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: initialized");
+     388             : 
+     389          42 :   is_initialized_ = true;
+     390          42 : }
+     391             : 
+     392             : //}
+     393             : 
+     394             : // | ---------------------- main routines --------------------- |
+     395             : 
+     396             : /*
+     397             :  * 1. preprocessPath(): preprocessing the incoming path
+     398             :  *    - throughs away too close waypoints
+     399             :  *    - straightness path by neglecting waypoints close to segments
+     400             :  * 2. optimize(): solves the whole problem including
+     401             :  *    - subdivision for satisfying max deviation
+     402             :  * 3. findTrajectory(): solves single instance by the ETH tool
+     403             :  * 4. findTrajectoryFallback(): Baca's sampling for backup solution
+     404             :  * 5. validateTrajectorySpatial(): checks for the spatial soundness of a trajectory vs. the original path
+     405             :  */
+     406             : 
+     407             : /* preprocessPath() //{ */
+     408             : 
+     409           1 : std::vector<Waypoint_t> MrsTrajectoryGeneration::preprocessPath(const std::vector<Waypoint_t>& waypoints_in) {
+     410             : 
+     411           2 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::preprocessPath", scope_timer_logger_, scope_timer_enabled_);
+     412             : 
+     413           1 :   std::vector<Waypoint_t> waypoints;
+     414             : 
+     415           1 :   size_t last_added_idx = 0;  // in "waypoints_in"
+     416             : 
+     417           3 :   for (size_t i = 0; i < waypoints_in.size(); i++) {
+     418             : 
+     419           2 :     double x       = waypoints_in.at(i).coords[0];
+     420           2 :     double y       = waypoints_in.at(i).coords[1];
+     421           2 :     double z       = waypoints_in.at(i).coords[2];
+     422           2 :     double heading = waypoints_in.at(i).coords[3];
+     423             : 
+     424           2 :     bw_original_.addPoint(vec3_t(x, y, z), 1.0, 0.0, 0.0, 1.0);
+     425             : 
+     426           2 :     if (_path_straightener_enabled_ && waypoints_in.size() >= 3 && i > 0 && i < (waypoints_in.size() - 1)) {
+     427             : 
+     428           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]);
+     429           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]);
+     430             : 
+     431           0 :       double first_hdg = waypoints_in.at(last_added_idx).coords[3];
+     432           0 :       double last_hdg  = waypoints_in.at(i + 1).coords[3];
+     433             : 
+     434           0 :       size_t next_point = last_added_idx + 1;
+     435             : 
+     436           0 :       bool segment_is_ok = true;
+     437             : 
+     438           0 :       for (size_t j = next_point; j < i + 1; j++) {
+     439             : 
+     440           0 :         vec3_t mid(waypoints_in.at(j).coords[0], waypoints_in.at(j).coords[1], waypoints_in.at(j).coords[2]);
+     441           0 :         double mid_hdg = waypoints_in.at(j).coords[3];
+     442             : 
+     443           0 :         double dist_from_segment = distFromSegment(mid, first, last);
+     444             : 
+     445           0 :         if (dist_from_segment > _path_straightener_max_deviation_ || fabs(radians::diff(first_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_) ||
+     446           0 :             fabs(radians::diff(last_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_)) {
+     447           0 :           segment_is_ok = false;
+     448           0 :           break;
+     449             :         }
+     450             :       }
+     451             : 
+     452           0 :       if (segment_is_ok) {
+     453           0 :         continue;
+     454             :       }
+     455             :     }
+     456             : 
+     457           2 :     if (i > 0 && i < (waypoints_in.size() - 1)) {
+     458             : 
+     459           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]);
+     460           0 :       vec3_t last(waypoints_in.at(i).coords[0], waypoints_in.at(i).coords[1], waypoints_in.at(i).coords[2]);
+     461             : 
+     462           0 :       if (mrs_lib::geometry::dist(first, last) < _min_waypoint_distance_) {
+     463           0 :         ROS_INFO("[MrsTrajectoryGeneration]: waypoint #%d too close (< %.3f m) to the previous one (#%d), throwing it away", int(i), _min_waypoint_distance_,
+     464             :                  int(last_added_idx));
+     465           0 :         continue;
+     466             :       }
+     467             :     }
+     468             : 
+     469           2 :     Waypoint_t wp;
+     470           2 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+     471           2 :     wp.stop_at = waypoints_in.at(i).stop_at;
+     472           2 :     waypoints.push_back(wp);
+     473             : 
+     474           2 :     last_added_idx = i;
+     475             :   }
+     476             : 
+     477           1 :   return waypoints;
+     478             : }
+     479             : 
+     480             : //}
+     481             : 
+     482             : /* optimize() //{ */
+     483             : 
+     484           1 : std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> MrsTrajectoryGeneration::optimize(const std::vector<Waypoint_t>&          waypoints_in,
+     485             :                                                                                                const std_msgs::Header&                 waypoints_header,
+     486             :                                                                                                const mrs_msgs::TrackerCommand&         tracker_cmd,
+     487             :                                                                                                const mrs_msgs::MpcPredictionFullState& current_prediction,
+     488             :                                                                                                const bool fallback_sampling, const bool relax_heading) {
+     489             : 
+     490           3 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::optimize", scope_timer_logger_, scope_timer_enabled_);
+     491             : 
+     492           1 :   ros::Time optimize_time_start = ros::Time::now();
+     493             : 
+     494             :   // | ------------- prepare the initial conditions ------------- |
+     495             : 
+     496           2 :   mrs_msgs::TrackerCommand initial_condition;
+     497             : 
+     498           1 :   bool path_from_future = false;
+     499             : 
+     500             :   // positive = in the future
+     501           1 :   double path_time_offset = 0;
+     502             : 
+     503           1 :   if (waypoints_header.stamp != ros::Time(0)) {
+     504           0 :     path_time_offset = (waypoints_header.stamp - ros::Time::now()).toSec();
+     505             :   }
+     506             : 
+     507           1 :   int path_sample_offset = 0;
+     508             : 
+     509             :   // if the desired path starts in the future, more than one MPC step ahead
+     510           1 :   if (path_time_offset > 0.2) {
+     511             : 
+     512           0 :     ROS_INFO("[MrsTrajectoryGeneration]: desired path is from the future by %.2f s", path_time_offset);
+     513             : 
+     514             :     // calculate the offset in samples in the predicted trajectory
+     515             :     // 0.01 is subtracted for the first sample, which is smaller
+     516             :     // +1 is added due to the first sample, which was subtarcted
+     517           0 :     path_sample_offset = int(ceil((path_time_offset * FUTURIZATION_FIRST_WAYPOINT_FACTOR - 0.01) / 0.2)) + 1;
+     518             : 
+     519           0 :     if (path_sample_offset > (int(current_prediction.position.size()) - 1)) {
+     520             : 
+     521           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: can not extrapolate into the waypoints, using tracker_cmd instead");
+     522           0 :       initial_condition = tracker_cmd;
+     523             : 
+     524             :     } else {
+     525             : 
+     526             :       // copy the sample from the current prediction into TrackerCommand, so that we can easily transform it
+     527           0 :       mrs_msgs::TrackerCommand full_state;
+     528             : 
+     529           0 :       full_state.header = current_prediction.header;
+     530             : 
+     531           0 :       full_state.position     = current_prediction.position[path_sample_offset];
+     532           0 :       full_state.velocity     = current_prediction.velocity[path_sample_offset];
+     533           0 :       full_state.acceleration = current_prediction.acceleration[path_sample_offset];
+     534           0 :       full_state.jerk         = current_prediction.jerk[path_sample_offset];
+     535             : 
+     536           0 :       full_state.heading              = current_prediction.heading[path_sample_offset];
+     537           0 :       full_state.heading_rate         = current_prediction.heading_rate[path_sample_offset];
+     538           0 :       full_state.heading_acceleration = current_prediction.heading_acceleration[path_sample_offset];
+     539           0 :       full_state.heading_jerk         = current_prediction.heading_jerk[path_sample_offset];
+     540             : 
+     541           0 :       ROS_INFO("[MrsTrajectoryGeneration]: getting initial condition from the %d-th sample of the MPC prediction", path_sample_offset);
+     542             : 
+     543           0 :       initial_condition.header = full_state.header;
+     544             : 
+     545           0 :       initial_condition.position     = full_state.position;
+     546           0 :       initial_condition.velocity     = full_state.velocity;
+     547           0 :       initial_condition.acceleration = full_state.acceleration;
+     548           0 :       initial_condition.jerk         = full_state.jerk;
+     549             : 
+     550           0 :       initial_condition.heading              = full_state.heading;
+     551           0 :       initial_condition.heading_rate         = full_state.heading_rate;
+     552           0 :       initial_condition.heading_acceleration = full_state.heading_acceleration;
+     553           0 :       initial_condition.heading_jerk         = full_state.heading_jerk;
+     554             : 
+     555           0 :       path_from_future = true;
+     556             :     }
+     557             : 
+     558             :   } else {
+     559             : 
+     560           2 :     ROS_INFO("[MrsTrajectoryGeneration]: desired path is NOT from the future, using tracker_cmd as the initial condition");
+     561             : 
+     562           1 :     initial_condition = tracker_cmd;
+     563             :   }
+     564             : 
+     565           1 :   auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_);
+     566             : 
+     567           1 :   if (waypoints_header.stamp == ros::Time(0)) {
+     568           1 :     if (!control_manager_diag.tracker_status.have_goal) {
+     569           1 :       initial_condition.header.stamp = ros::Time(0);
+     570             :     }
+     571             :   }
+     572             : 
+     573             :   // | ---------------- reset the visual markers ---------------- |
+     574             : 
+     575           1 :   bw_original_.clearBuffers();
+     576           1 :   bw_original_.clearVisuals();
+     577           1 :   bw_final_.clearBuffers();
+     578           1 :   bw_final_.clearVisuals();
+     579             : 
+     580           1 :   bw_original_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     581           1 :   bw_final_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     582             : 
+     583           1 :   bw_original_.setPointsScale(0.4);
+     584           1 :   bw_final_.setPointsScale(0.35);
+     585             : 
+     586             :   // empty path is invalid
+     587           1 :   if (waypoints_in.size() == 0) {
+     588           0 :     std::stringstream ss;
+     589           0 :     ss << "the path is empty (before postprocessing)";
+     590           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     591           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     592             :   }
+     593             : 
+     594           2 :   std::vector<Waypoint_t> waypoints_in_with_init = waypoints_in;
+     595             : 
+     596           1 :   if (path_time_offset > 0.2 && waypoints_in_with_init.size() >= 2) {
+     597           0 :     waypoints_in_with_init.erase(waypoints_in_with_init.begin());
+     598             :   }
+     599             : 
+     600             :   // prepend the initial condition
+     601           1 :   Waypoint_t initial_waypoint;
+     602           1 :   initial_waypoint.coords =
+     603           1 :       Eigen::Vector4d(initial_condition.position.x, initial_condition.position.y, initial_condition.position.z, initial_condition.heading);
+     604           1 :   initial_waypoint.stop_at = false;
+     605           1 :   waypoints_in_with_init.insert(waypoints_in_with_init.begin(), initial_waypoint);
+     606             : 
+     607           2 :   std::vector<Waypoint_t> waypoints = preprocessPath(waypoints_in_with_init);
+     608             : 
+     609           1 :   if (waypoints.size() <= 1) {
+     610           0 :     std::stringstream ss;
+     611           0 :     ss << "the path is empty (after postprocessing)";
+     612           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     613           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     614             :   }
+     615             : 
+     616           1 :   bool              safe = false;
+     617           1 :   int               traj_idx;
+     618           2 :   std::vector<bool> segment_safeness;
+     619           1 :   double            max_deviation;
+     620             : 
+     621           2 :   eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory;
+     622             : 
+     623           1 :   double sampling_dt = 0;
+     624             : 
+     625           1 :   if (path_from_future) {
+     626           0 :     ROS_INFO("[MrsTrajectoryGeneration]: changing dt = 0.2, cause the path is from the future");
+     627           0 :     sampling_dt = 0.2;
+     628             :   } else {
+     629           1 :     sampling_dt = _sampling_dt_;
+     630             :   }
+     631             : 
+     632           1 :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> result;
+     633             : 
+     634           2 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+     635             : 
+     636           1 :   if (params.enforce_fallback_solver) {
+     637           0 :     ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced");
+     638           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     639           1 :   } else if (fallback_sampling) {
+     640           0 :     ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling");
+     641           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     642           1 :   } else if (running_async_planning_) {
+     643           0 :     ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     644           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     645           1 :   } else if (overtime()) {
+     646           0 :     ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time");
+     647           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     648             :   } else {
+     649           3 :     result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     650             :   }
+     651             : 
+     652           1 :   if (result) {
+     653           1 :     trajectory = result.value();
+     654             :   } else {
+     655           0 :     std::stringstream ss;
+     656           0 :     ss << "failed to find trajectory";
+     657           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     658           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     659             :   }
+     660             : 
+     661           1 :   for (int k = 0; k < _trajectory_max_segment_deviation_max_iterations_; k++) {
+     662             : 
+     663           2 :     ROS_DEBUG("[MrsTrajectoryGeneration]: revalidation cycle #%d", k);
+     664             : 
+     665           1 :     std::tie(safe, traj_idx, segment_safeness, max_deviation) = validateTrajectorySpatial(trajectory, waypoints);
+     666             : 
+     667           1 :     if (_trajectory_max_segment_deviation_enabled_ && !safe) {
+     668             : 
+     669           0 :       ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is not safe, max deviation %.3f m", max_deviation);
+     670             : 
+     671           0 :       std::vector<Waypoint_t>::iterator waypoint = waypoints.begin();
+     672           0 :       std::vector<bool>::iterator       safeness = segment_safeness.begin();
+     673             : 
+     674           0 :       for (; waypoint < waypoints.end() - 1; waypoint++) {
+     675             : 
+     676           0 :         if (!(*safeness)) {
+     677             : 
+     678           0 :           if (waypoint > waypoints.begin() || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+     679           0 :             Waypoint_t midpoint2 = interpolatePoint(*waypoint, *(waypoint + 1), 0.5);
+     680           0 :             waypoint             = waypoints.insert(waypoint + 1, midpoint2);
+     681             :           }
+     682             :         }
+     683             : 
+     684           0 :         safeness++;
+     685             :       }
+     686             : 
+     687           0 :       if (params.enforce_fallback_solver) {
+     688           0 :         ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced");
+     689           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     690           0 :       } else if (fallback_sampling) {
+     691           0 :         ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling");
+     692           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     693           0 :       } else if (running_async_planning_) {
+     694           0 :         ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     695           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     696           0 :       } else if (overtime()) {
+     697           0 :         ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time");
+     698           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     699             :       } else {
+     700           0 :         result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     701             :       }
+     702             : 
+     703           0 :       if (result) {
+     704           0 :         trajectory = result.value();
+     705             :       } else {
+     706           0 :         std::stringstream ss;
+     707           0 :         ss << "failed to find trajectory";
+     708           0 :         ROS_WARN_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     709           0 :         return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     710             :       }
+     711             : 
+     712             :     } else {
+     713           2 :       ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is safe (%.2f)", max_deviation);
+     714           1 :       safe = true;
+     715           1 :       break;
+     716             :     }
+     717             :   }
+     718             : 
+     719           2 :   ROS_INFO("[MrsTrajectoryGeneration]: final max trajectory-path deviation: %.2f m, total trajectory time: %.2fs ", max_deviation,
+     720             :            trajectory.size() * sampling_dt);
+     721             : 
+     722             :   // prepare rviz markers
+     723           3 :   for (int i = 0; i < int(waypoints.size()); i++) {
+     724           2 :     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);
+     725             :   }
+     726             : 
+     727           2 :   mrs_msgs::TrajectoryReference mrs_trajectory;
+     728             : 
+     729             :   // convert the optimized trajectory to mrs_msgs::TrajectoryReference
+     730           1 :   mrs_trajectory = getTrajectoryReference(trajectory, initial_condition.header.stamp, sampling_dt);
+     731             : 
+     732             :   // insert part of the MPC prediction in the front of the generated trajectory to compensate for the future
+     733           1 :   if (path_from_future) {
+     734             : 
+     735             :     // calculate the starting idx that we will use from the current_prediction
+     736           0 :     double path_time_offset_2   = (ros::Time::now() - current_prediction.header.stamp).toSec();  // = how long did it take to optimize
+     737           0 :     int    path_sample_offset_2 = int(floor((path_time_offset_2 - 0.01) / 0.2)) + 1;
+     738             : 
+     739             :     // if there is anything to insert
+     740           0 :     if (path_sample_offset > path_sample_offset_2) {
+     741             : 
+     742           0 :       ROS_INFO("[MrsTrajectoryGeneration]: inserting pre-trajectory from the prediction, idxs %d to %d", path_sample_offset_2, path_sample_offset);
+     743             : 
+     744           0 :       for (int i = path_sample_offset - 1; i >= 0; i--) {
+     745             : 
+     746           0 :         ROS_DEBUG("[MrsTrajectoryGeneration]: inserting idx %d", i);
+     747             : 
+     748           0 :         mrs_msgs::ReferenceStamped reference;
+     749             : 
+     750           0 :         reference.header = current_prediction.header;
+     751             : 
+     752           0 :         reference.reference.heading  = current_prediction.heading[i];
+     753           0 :         reference.reference.position = current_prediction.position[i];
+     754             : 
+     755           0 :         auto res = transformer_->transformSingle(reference, waypoints_header.frame_id);
+     756             : 
+     757           0 :         if (res) {
+     758           0 :           reference = res.value();
+     759             :         } else {
+     760           0 :           std::stringstream ss;
+     761           0 :           ss << "could not transform reference to the path frame";
+     762           0 :           ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     763           0 :           return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     764             :         }
+     765             : 
+     766           0 :         mrs_trajectory.points.insert(mrs_trajectory.points.begin(), reference.reference);
+     767             :       }
+     768             :     }
+     769             :   }
+     770             : 
+     771           1 :   bw_original_.publish();
+     772           1 :   bw_final_.publish();
+     773             : 
+     774           2 :   std::stringstream ss;
+     775           1 :   ss << "trajectory generated";
+     776             : 
+     777           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory generated, took %.3f s", (ros::Time::now() - optimize_time_start).toSec());
+     778             : 
+     779           2 :   return std::tuple(true, ss.str(), mrs_trajectory);
+     780             : }
+     781             : 
+     782             : //}
+     783             : 
+     784             : /* findTrajectory() //{ */
+     785             : 
+     786           1 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectory(const std::vector<Waypoint_t>&  waypoints,
+     787             :                                                                                                   const mrs_msgs::TrackerCommand& initial_state,
+     788             :                                                                                                   const double& sampling_dt, const bool& relax_heading) {
+     789             : 
+     790           3 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectory", scope_timer_logger_, scope_timer_enabled_);
+     791             : 
+     792           1 :   mrs_lib::AtomicScopeFlag unset_running(running_async_planning_);
+     793             : 
+     794           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: findTrajectory() started");
+     795             : 
+     796           1 :   ros::Time find_trajectory_time_start = ros::Time::now();
+     797             : 
+     798           2 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+     799           1 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     800             : 
+     801           2 :   auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_);
+     802             : 
+     803           1 :   if ((initial_state.header.stamp - ros::Time::now()).toSec() < 0.2 && control_manager_diag.tracker_status.have_goal) {
+     804           0 :     max_deviation_first_segment_ = false;
+     805             :   } else {
+     806           1 :     max_deviation_first_segment_ = true;
+     807             :   }
+     808             : 
+     809             :   // optimizer
+     810             : 
+     811           1 :   eth_trajectory_generation::NonlinearOptimizationParameters parameters;
+     812             : 
+     813           1 :   parameters.f_rel                  = 0.05;
+     814           1 :   parameters.x_rel                  = 0.1;
+     815           1 :   parameters.time_penalty           = params.time_penalty;
+     816           1 :   parameters.use_soft_constraints   = params.soft_constraints_enabled;
+     817           1 :   parameters.soft_constraint_weight = params.soft_constraints_weight;
+     818           1 :   parameters.time_alloc_method      = static_cast<eth_trajectory_generation::NonlinearOptimizationParameters::TimeAllocMethod>(params.time_allocation);
+     819           1 :   if (params.time_allocation == 2) {
+     820           1 :     parameters.algorithm = nlopt::LD_LBFGS;
+     821             :   }
+     822           1 :   parameters.initial_stepsize_rel            = 0.1;
+     823           1 :   parameters.inequality_constraint_tolerance = params.inequality_constraint_tolerance;
+     824           1 :   parameters.equality_constraint_tolerance   = params.equality_constraint_tolerance;
+     825           1 :   parameters.max_iterations                  = params.max_iterations;
+     826           1 :   parameters.max_time                        = NLOPT_EXEC_TIME_FACTOR * timeLeft();
+     827             : 
+     828           1 :   eth_trajectory_generation::Vertex::Vector vertices;
+     829           1 :   const int                                 dimension = 4;
+     830             : 
+     831           1 :   int derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     832             : 
+     833           1 :   switch (params.derivative_to_optimize) {
+     834             :     case 0: {
+     835             :       derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     836             :       break;
+     837             :     }
+     838           0 :     case 1: {
+     839           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::JERK;
+     840           0 :       break;
+     841             :     }
+     842           0 :     case 2: {
+     843           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::SNAP;
+     844           0 :       break;
+     845             :     }
+     846             :   }
+     847             : 
+     848             :   // | --------------- add constraints to vertices -------------- |
+     849             : 
+     850           1 :   double last_heading = initial_state.heading;
+     851             : 
+     852           3 :   for (size_t i = 0; i < waypoints.size(); i++) {
+     853           2 :     double x       = waypoints.at(i).coords[0];
+     854           2 :     double y       = waypoints.at(i).coords[1];
+     855           2 :     double z       = waypoints.at(i).coords[2];
+     856           2 :     double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading);
+     857           2 :     last_heading   = heading;
+     858             : 
+     859           4 :     eth_trajectory_generation::Vertex vertex(dimension);
+     860             : 
+     861           2 :     if (i == 0) {
+     862             : 
+     863           2 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     864             : 
+     865           2 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     866             : 
+     867           1 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY,
+     868           1 :                            Eigen::Vector4d(initial_state.velocity.x, initial_state.velocity.y, initial_state.velocity.z, initial_state.heading_rate));
+     869             : 
+     870           1 :       vertex.addConstraint(
+     871             :           eth_trajectory_generation::derivative_order::ACCELERATION,
+     872           1 :           Eigen::Vector4d(initial_state.acceleration.x, initial_state.acceleration.y, initial_state.acceleration.z, initial_state.heading_acceleration));
+     873             : 
+     874           1 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK,
+     875           2 :                            Eigen::Vector4d(initial_state.jerk.x, initial_state.jerk.y, initial_state.jerk.z, initial_state.heading_jerk));
+     876             : 
+     877           1 :     } else if (i == (waypoints.size() - 1)) {  // the last point
+     878             : 
+     879           2 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     880             : 
+     881           2 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     882             : 
+     883             :     } else {  // mid points
+     884             : 
+     885           0 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     886             : 
+     887           0 :       if (waypoints.at(i).stop_at) {
+     888           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector4d(0, 0, 0, 0));
+     889           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, Eigen::Vector4d(0, 0, 0, 0));
+     890           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, Eigen::Vector4d(0, 0, 0, 0));
+     891             :       }
+     892             :     }
+     893             : 
+     894           2 :     vertices.push_back(vertex);
+     895             :   }
+     896             : 
+     897             :   // | ---------------- compute the segment times --------------- |
+     898             : 
+     899           1 :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+     900           1 :   double v_max_vertical, a_max_vertical, j_max_vertical;
+     901             : 
+     902             :   // use the small of the ascending/descending values
+     903           1 :   double vertical_speed_lim        = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed);
+     904           1 :   double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration);
+     905             : 
+     906           1 :   if (override_constraints_) {
+     907             : 
+     908           0 :     bool can_change = (hypot(initial_state.velocity.x, initial_state.velocity.y) < override_max_velocity_horizontal_) &&
+     909           0 :                       (hypot(initial_state.acceleration.x, initial_state.acceleration.y) < override_max_acceleration_horizontal_) &&
+     910           0 :                       (hypot(initial_state.jerk.x, initial_state.jerk.y) < override_max_jerk_horizontal_) &&
+     911           0 :                       (fabs(initial_state.velocity.z) < override_max_velocity_vertical_) &&
+     912           0 :                       (fabs(initial_state.acceleration.z) < override_max_acceleration_vertical_) && (fabs(initial_state.jerk.z) < override_max_jerk_vertical_);
+     913             : 
+     914           0 :     if (can_change) {
+     915             : 
+     916           0 :       v_max_horizontal = override_max_velocity_horizontal_;
+     917           0 :       a_max_horizontal = override_max_acceleration_horizontal_;
+     918           0 :       j_max_horizontal = override_max_jerk_horizontal_;
+     919             : 
+     920           0 :       v_max_vertical = override_max_velocity_vertical_;
+     921           0 :       a_max_vertical = override_max_acceleration_vertical_;
+     922           0 :       j_max_vertical = override_max_jerk_vertical_;
+     923             : 
+     924           0 :       ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user");
+     925             : 
+     926             :     } else {
+     927             : 
+     928           0 :       ROS_WARN("[MrsTrajectoryGeneration]: overrifing constraints refused due to possible infeasibility");
+     929             :     }
+     930             : 
+     931             :   } else {
+     932             : 
+     933           1 :     v_max_horizontal = constraints.horizontal_speed;
+     934           1 :     a_max_horizontal = constraints.horizontal_acceleration;
+     935             : 
+     936           1 :     v_max_vertical = vertical_speed_lim;
+     937           1 :     a_max_vertical = vertical_acceleration_lim;
+     938             : 
+     939           1 :     j_max_horizontal = constraints.horizontal_jerk;
+     940           1 :     j_max_vertical   = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk);
+     941             :   }
+     942             : 
+     943             : 
+     944           1 :   double v_max_heading, a_max_heading, j_max_heading;
+     945             : 
+     946           1 :   if (relax_heading) {
+     947             :     v_max_heading = std::numeric_limits<float>::max();
+     948             :     a_max_heading = std::numeric_limits<float>::max();
+     949             :     j_max_heading = std::numeric_limits<float>::max();
+     950             :   } else {
+     951           1 :     v_max_heading = constraints.heading_speed;
+     952           1 :     a_max_heading = constraints.heading_acceleration;
+     953           1 :     j_max_heading = constraints.heading_jerk;
+     954             :   }
+     955             : 
+     956           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:");
+     957           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+     958           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+     959           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+     960             : 
+     961           3 :   std::vector<double> segment_times, segment_times_baca;
+     962           1 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+     963           1 :                                        v_max_heading, a_max_heading);
+     964           1 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+     965           1 :                                                 v_max_heading, a_max_heading);
+     966             : 
+     967           1 :   double initial_total_time      = 0;
+     968           1 :   double initial_total_time_baca = 0;
+     969           2 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+     970           1 :     initial_total_time += segment_times[i];
+     971           1 :     initial_total_time_baca += segment_times_baca[i];
+     972             :   }
+     973             : 
+     974           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Euclidean): %.2f", initial_total_time);
+     975           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Baca): %.2f", initial_total_time_baca);
+     976             : 
+     977             :   // | --------- create an optimizer object and solve it -------- |
+     978             : 
+     979           1 :   const int                                                     N = 10;
+     980           2 :   eth_trajectory_generation::PolynomialOptimizationNonLinear<N> opt(dimension, parameters);
+     981           1 :   opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
+     982             : 
+     983           1 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+     984           1 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+     985           1 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+     986             : 
+     987           1 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+     988           1 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+     989           1 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+     990             : 
+     991           1 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::VELOCITY, v_max_vertical);
+     992           1 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_vertical);
+     993           1 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::JERK, j_max_vertical);
+     994             : 
+     995           1 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::VELOCITY, v_max_heading);
+     996           1 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_heading);
+     997           1 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::JERK, j_max_heading);
+     998             : 
+     999           1 :   opt.optimize();
+    1000             : 
+    1001           1 :   if (overtime()) {
+    1002           0 :     return {};
+    1003             :   }
+    1004             : 
+    1005           2 :   std::string result_str;
+    1006             : 
+    1007           2 :   switch (opt.getOptimizationInfo().stopping_reason) {
+    1008           0 :     case nlopt::FAILURE: {
+    1009           0 :       result_str = "generic failure";
+    1010             :       break;
+    1011             :     }
+    1012           0 :     case nlopt::INVALID_ARGS: {
+    1013           0 :       result_str = "invalid args";
+    1014             :       break;
+    1015             :     }
+    1016           0 :     case nlopt::OUT_OF_MEMORY: {
+    1017           0 :       result_str = "out of memory";
+    1018             :       break;
+    1019             :     }
+    1020           0 :     case nlopt::ROUNDOFF_LIMITED: {
+    1021           0 :       result_str = "roundoff limited";
+    1022             :       break;
+    1023             :     }
+    1024           0 :     case nlopt::FORCED_STOP: {
+    1025           0 :       result_str = "forced stop";
+    1026             :       break;
+    1027             :     }
+    1028           0 :     case nlopt::STOPVAL_REACHED: {
+    1029           0 :       result_str = "stopval reached";
+    1030             :       break;
+    1031             :     }
+    1032           0 :     case nlopt::FTOL_REACHED: {
+    1033           0 :       result_str = "ftol reached";
+    1034             :       break;
+    1035             :     }
+    1036           0 :     case nlopt::XTOL_REACHED: {
+    1037           0 :       result_str = "xtol reached";
+    1038             :       break;
+    1039             :     }
+    1040           0 :     case nlopt::MAXEVAL_REACHED: {
+    1041           0 :       result_str = "maxeval reached";
+    1042             :       break;
+    1043             :     }
+    1044           0 :     case nlopt::MAXTIME_REACHED: {
+    1045           0 :       result_str = "maxtime reached";
+    1046             :       break;
+    1047             :     }
+    1048           1 :     default: {
+    1049           1 :       result_str = "UNKNOWN FAILURE CODE";
+    1050             :       break;
+    1051             :     }
+    1052             :   }
+    1053             : 
+    1054           4 :   if (opt.getOptimizationInfo().stopping_reason >= 1 && opt.getOptimizationInfo().stopping_reason != 6) {
+    1055           2 :     ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished successfully with code %d, '%s'", opt.getOptimizationInfo().stopping_reason,
+    1056             :               result_str.c_str());
+    1057             : 
+    1058           0 :   } else if (opt.getOptimizationInfo().stopping_reason == -1) {
+    1059           0 :     ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished with a generic error code %d, '%s'", opt.getOptimizationInfo().stopping_reason,
+    1060             :               result_str.c_str());
+    1061             : 
+    1062             :   } else {
+    1063           0 :     ROS_WARN("[MrsTrajectoryGeneration]: optimization failed with code %d, '%s', took %.3f s", opt.getOptimizationInfo().stopping_reason, result_str.c_str(),
+    1064             :              (ros::Time::now() - find_trajectory_time_start).toSec());
+    1065           1 :     return {};
+    1066             :   }
+    1067             : 
+    1068             :   // | ------------- obtain the polynomial segments ------------- |
+    1069             : 
+    1070           1 :   eth_trajectory_generation::Segment::Vector segments;
+    1071           1 :   opt.getPolynomialOptimizationRef().getSegments(&segments);
+    1072             : 
+    1073           1 :   if (overtime()) {
+    1074           0 :     return {};
+    1075             :   }
+    1076             : 
+    1077             :   // | --------------- create the trajectory class -------------- |
+    1078             : 
+    1079           2 :   eth_trajectory_generation::Trajectory trajectory;
+    1080           1 :   opt.getTrajectory(&trajectory);
+    1081             : 
+    1082           2 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1083             : 
+    1084           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: starting eth sampling with dt = %.2f s ", sampling_dt);
+    1085             : 
+    1086           1 :   bool success = eth_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_dt, &states);
+    1087             : 
+    1088           1 :   if (overtime()) {
+    1089           0 :     return {};
+    1090             :   }
+    1091             : 
+    1092             :   // validate the temporal sampling of the trajectory
+    1093             : 
+    1094             :   // only check this if the trajectory is > 1.0 sec, this check does not make much sense for the short ones
+    1095           1 :   if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) > (_max_trajectory_len_factor_ * initial_total_time_baca)) {
+    1096           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too long = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1097             :               (states.size() * sampling_dt), initial_total_time_baca, _max_trajectory_len_factor_);
+    1098             : 
+    1099           0 :     std::stringstream ss;
+    1100           0 :     ss << "trajectory sampling failed";
+    1101           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+    1102           0 :     return {};
+    1103             : 
+    1104           1 :   } else if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) < (_min_trajectory_len_factor_ * initial_total_time_baca)) {
+    1105           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too short = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1106             :               (states.size() * sampling_dt), initial_total_time_baca, _min_trajectory_len_factor_);
+    1107             : 
+    1108           0 :     std::stringstream ss;
+    1109           0 :     ss << "trajectory sampling failed";
+    1110           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+    1111           0 :     return {};
+    1112             : 
+    1113             :   } else {
+    1114           2 :     ROS_DEBUG("[MrsTrajectoryGeneration]: estimated/final trajectory length ratio (final/estimated) %.2f",
+    1115             :               (states.size() * sampling_dt) / initial_total_time_baca);
+    1116             :   }
+    1117             : 
+    1118           1 :   if (success) {
+    1119           2 :     ROS_DEBUG("[MrsTrajectoryGeneration]: eth sampling finished, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1120           1 :     return std::optional(states);
+    1121             : 
+    1122             :   } else {
+    1123           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: eth could not sample the trajectory, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1124           1 :     return {};
+    1125             :   }
+    1126             : }
+    1127             : 
+    1128             : //}
+    1129             : 
+    1130             : /* findTrajectoryFallback() //{ */
+    1131             : 
+    1132           0 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryFallback(const std::vector<Waypoint_t>&  waypoints,
+    1133             :                                                                                                           const mrs_msgs::TrackerCommand& initial_state,
+    1134             :                                                                                                           const double&                   sampling_dt,
+    1135             :                                                                                                           const bool&                     relax_heading) {
+    1136             : 
+    1137           0 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectoryFallback", scope_timer_logger_, scope_timer_enabled_);
+    1138             : 
+    1139           0 :   ros::Time time_start = ros::Time::now();
+    1140             : 
+    1141           0 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling started");
+    1142             : 
+    1143           0 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+    1144           0 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1145             : 
+    1146           0 :   eth_trajectory_generation::Vertex::Vector vertices;
+    1147           0 :   const int                                 dimension = 4;
+    1148             : 
+    1149             :   // | --------------- add constraints to vertices -------------- |
+    1150             : 
+    1151           0 :   double last_heading = initial_state.heading;
+    1152             : 
+    1153           0 :   for (size_t i = 0; i < waypoints.size(); i++) {
+    1154             : 
+    1155           0 :     double x       = waypoints.at(i).coords[0];
+    1156           0 :     double y       = waypoints.at(i).coords[1];
+    1157           0 :     double z       = waypoints.at(i).coords[2];
+    1158           0 :     double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading);
+    1159           0 :     last_heading   = heading;
+    1160             : 
+    1161           0 :     eth_trajectory_generation::Vertex vertex(dimension);
+    1162             : 
+    1163           0 :     vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+    1164             : 
+    1165           0 :     vertices.push_back(vertex);
+    1166             :   }
+    1167             : 
+    1168             :   // | ---------------- compute the segment times --------------- |
+    1169             : 
+    1170           0 :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+    1171           0 :   double v_max_vertical, a_max_vertical, j_max_vertical;
+    1172             : 
+    1173             :   // use the small of the ascending/descending values
+    1174           0 :   double vertical_speed_lim        = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed);
+    1175           0 :   double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration);
+    1176             : 
+    1177           0 :   if (override_constraints_) {
+    1178             : 
+    1179           0 :     v_max_horizontal = override_max_velocity_horizontal_;
+    1180           0 :     a_max_horizontal = override_max_acceleration_horizontal_;
+    1181           0 :     j_max_horizontal = override_max_jerk_horizontal_;
+    1182             : 
+    1183           0 :     v_max_vertical = override_max_velocity_vertical_;
+    1184           0 :     a_max_vertical = override_max_acceleration_vertical_;
+    1185           0 :     j_max_vertical = override_max_jerk_vertical_;
+    1186             : 
+    1187           0 :     ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user");
+    1188             :   } else {
+    1189             : 
+    1190           0 :     v_max_horizontal = constraints.horizontal_speed;
+    1191           0 :     a_max_horizontal = constraints.horizontal_acceleration;
+    1192             : 
+    1193           0 :     v_max_vertical = vertical_speed_lim;
+    1194           0 :     a_max_vertical = vertical_acceleration_lim;
+    1195             : 
+    1196           0 :     j_max_horizontal = constraints.horizontal_jerk;
+    1197           0 :     j_max_vertical   = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk);
+    1198             :   }
+    1199             : 
+    1200             : 
+    1201           0 :   double v_max_heading, a_max_heading, j_max_heading;
+    1202             : 
+    1203           0 :   if (relax_heading) {
+    1204             :     v_max_heading = std::numeric_limits<float>::max();
+    1205             :     a_max_heading = std::numeric_limits<float>::max();
+    1206             :     j_max_heading = std::numeric_limits<float>::max();
+    1207             :   } else {
+    1208           0 :     v_max_heading = constraints.heading_speed;
+    1209           0 :     a_max_heading = constraints.heading_acceleration;
+    1210           0 :     j_max_heading = constraints.heading_jerk;
+    1211             :   }
+    1212             : 
+    1213           0 :   v_max_horizontal *= _fallback_sampling_speed_factor_;
+    1214           0 :   v_max_vertical *= _fallback_sampling_speed_factor_;
+    1215             : 
+    1216           0 :   a_max_horizontal *= _fallback_sampling_accel_factor_;
+    1217           0 :   a_max_vertical *= _fallback_sampling_accel_factor_;
+    1218             : 
+    1219           0 :   ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:");
+    1220           0 :   ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+    1221           0 :   ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+    1222           0 :   ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+    1223             : 
+    1224           0 :   std::vector<double> segment_times, segment_times_baca;
+    1225           0 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1226           0 :                                        v_max_heading, a_max_heading);
+    1227           0 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1228           0 :                                                 v_max_heading, a_max_heading);
+    1229             : 
+    1230           0 :   double initial_total_time      = 0;
+    1231           0 :   double initial_total_time_baca = 0;
+    1232           0 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+    1233           0 :     initial_total_time += segment_times[i];
+    1234           0 :     initial_total_time_baca += segment_times_baca[i];
+    1235             :   }
+    1236             : 
+    1237           0 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time);
+    1238           0 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca);
+    1239             : 
+    1240             :   // | --------- create an optimizer object and solve it -------- |
+    1241             : 
+    1242           0 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1243             : 
+    1244             :   // interpolate each segment
+    1245           0 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1246             : 
+    1247           0 :     Eigen::VectorXd start, end;
+    1248             : 
+    1249           0 :     const double segment_time = segment_times_baca[i];
+    1250             : 
+    1251           0 :     int    n_samples;
+    1252           0 :     double interp_step;
+    1253             : 
+    1254           0 :     if (segment_time > 1e-1) {
+    1255             : 
+    1256           0 :       n_samples = floor(segment_time / sampling_dt);
+    1257             : 
+    1258             :       // important
+    1259           0 :       if (n_samples > 0) {
+    1260           0 :         interp_step = 1.0 / double(n_samples);
+    1261             :       } else {
+    1262             :         interp_step = 0.5;
+    1263             :       }
+    1264             : 
+    1265             :     } else {
+    1266             :       n_samples   = 0;
+    1267             :       interp_step = 0;
+    1268             :     }
+    1269             : 
+    1270             :     // for the last segment, git the last waypoint completely
+    1271             :     // otherwise, it is hit as the first sample of the following segment
+    1272           0 :     if (n_samples > 0 && i == waypoints.size() - 2) {
+    1273           0 :       n_samples++;
+    1274             :     }
+    1275             : 
+    1276           0 :     for (int j = 0; j < n_samples; j++) {
+    1277             : 
+    1278           0 :       Waypoint_t point = interpolatePoint(waypoints[i], waypoints[i + 1], j * interp_step);
+    1279             : 
+    1280           0 :       eth_mav_msgs::EigenTrajectoryPoint eth_point;
+    1281           0 :       eth_point.position_W[0] = point.coords[0];
+    1282           0 :       eth_point.position_W[1] = point.coords[1];
+    1283           0 :       eth_point.position_W[2] = point.coords[2];
+    1284           0 :       eth_point.setFromYaw(point.coords[3]);
+    1285             : 
+    1286           0 :       states.push_back(eth_point);
+    1287             : 
+    1288           0 :       auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_);
+    1289             : 
+    1290             :       // the first sample of the first waypoint
+    1291             :       // we should stop for a little bit to give the transition
+    1292             :       // from the initial cooordinates more time
+    1293           0 :       if (((control_manager_diag.tracker_status.have_goal && i == 0) || (!control_manager_diag.tracker_status.have_goal && i == 1)) && j == 0) {
+    1294             : 
+    1295           0 :         double time_to_stop = 0;
+    1296             : 
+    1297           0 :         time_to_stop += fabs(initial_state.acceleration.x) / j_max_horizontal + fabs(initial_state.acceleration.y) / j_max_horizontal +
+    1298           0 :                         fabs(initial_state.acceleration.z) / j_max_vertical;
+    1299             : 
+    1300           0 :         int samples_to_stop = int(round(1.0 * (time_to_stop / sampling_dt)));
+    1301             : 
+    1302           0 :         if (_fallback_sampling_first_waypoint_additional_stop_) {
+    1303           0 :           if (control_manager_diag.tracker_status.have_goal) {
+    1304           0 :             samples_to_stop += int(round(1.0 / sampling_dt));
+    1305             :           }
+    1306             :         }
+    1307             : 
+    1308           0 :         ROS_DEBUG("[MrsTrajectoryGeneration]: pre-inserting %d samples of the first point", samples_to_stop);
+    1309             : 
+    1310           0 :         for (int k = 0; k < samples_to_stop; k++) {
+    1311           0 :           states.push_back(eth_point);
+    1312             :         }
+    1313             :       }
+    1314             : 
+    1315           0 :       if (j == 0 && i > 0 && waypoints[i].stop_at) {
+    1316             : 
+    1317           0 :         int insert_samples = int(round(_fallback_sampling_stopping_time_ / sampling_dt));
+    1318             : 
+    1319           0 :         for (int k = 0; k < insert_samples; k++) {
+    1320           0 :           states.push_back(eth_point);
+    1321             :         }
+    1322             :       }
+    1323             :     }
+    1324             :   }
+    1325             : 
+    1326           0 :   bool success = true;
+    1327             : 
+    1328           0 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback: sampling finished, took %.3f s", (ros::Time::now() - time_start).toSec());
+    1329             : 
+    1330             :   // | --------------- create the trajectory class -------------- |
+    1331             : 
+    1332           0 :   if (success) {
+    1333           0 :     return std::optional(states);
+    1334             :   } else {
+    1335             :     ROS_ERROR("[MrsTrajectoryGeneration]: fallback: sampling failed");
+    1336           0 :     return {};
+    1337             :   }
+    1338             : }
+    1339             : 
+    1340             : //}
+    1341             : 
+    1342             : /* validateTrajectorySpatial() //{ */
+    1343             : 
+    1344           1 : std::tuple<bool, int, std::vector<bool>, double> MrsTrajectoryGeneration::validateTrajectorySpatial(
+    1345             :     const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector<Waypoint_t>& waypoints) {
+    1346             : 
+    1347           2 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::validateTrajectorySpatial", scope_timer_logger_, scope_timer_enabled_);
+    1348             : 
+    1349             :   // prepare the output
+    1350             : 
+    1351           2 :   std::vector<bool> segments;
+    1352           2 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1353           1 :     segments.push_back(true);
+    1354             :   }
+    1355             : 
+    1356             :   int waypoint_idx = 0;
+    1357             : 
+    1358             :   bool   is_safe       = true;
+    1359             :   double max_deviation = 0;
+    1360             : 
+    1361          40 :   for (size_t i = 0; i < trajectory.size() - 1; i++) {
+    1362             : 
+    1363             :     // the trajectory sample
+    1364          39 :     const vec3_t sample = vec3_t(trajectory[i].position_W[0], trajectory[i].position_W[1], trajectory[i].position_W[2]);
+    1365             : 
+    1366             :     // next sample
+    1367          39 :     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]);
+    1368             : 
+    1369             :     // segment start
+    1370          39 :     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]);
+    1371             : 
+    1372             :     // segment end
+    1373          39 :     const vec3_t segment_end =
+    1374          39 :         vec3_t(waypoints.at(waypoint_idx + 1).coords[0], waypoints.at(waypoint_idx + 1).coords[1], waypoints.at(waypoint_idx + 1).coords[2]);
+    1375             : 
+    1376          39 :     const double distance_from_segment = distFromSegment(sample, segment_start, segment_end);
+    1377             : 
+    1378          39 :     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
+    1379             : 
+    1380          39 :     if (waypoint_idx > 0 || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+    1381             : 
+    1382          39 :       if (distance_from_segment > max_deviation) {
+    1383          10 :         max_deviation = distance_from_segment;
+    1384             :       }
+    1385             : 
+    1386          39 :       if (distance_from_segment > _trajectory_max_segment_deviation_) {
+    1387           0 :         segments.at(waypoint_idx) = false;
+    1388           0 :         is_safe                   = false;
+    1389             :       }
+    1390             :     }
+    1391             : 
+    1392          39 :     if (segment_end_dist < 0.05 && waypoint_idx < (int(waypoints.size()) - 2)) {
+    1393             :       waypoint_idx++;
+    1394             :     }
+    1395             :   }
+    1396             : 
+    1397           2 :   return std::tuple(is_safe, trajectory.size(), segments, max_deviation);
+    1398             : }
+    1399             : 
+    1400             : //}
+    1401             : 
+    1402             : /* getTrajectorySegmentCenterIdxs() //{ */
+    1403             : 
+    1404           0 : std::vector<int> MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+    1405             :                                                                          const std::vector<Waypoint_t>&                    waypoints) {
+    1406             : 
+    1407           0 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs", scope_timer_logger_, scope_timer_enabled_);
+    1408             : 
+    1409             :   // prepare the output
+    1410             : 
+    1411           0 :   std::vector<int> segment_centers;
+    1412             : 
+    1413             :   // index in the path
+    1414           0 :   int last_segment_start = 0;
+    1415             : 
+    1416             :   // index in the trajectory
+    1417           0 :   int last_segment_start_sample = 0;
+    1418             : 
+    1419           0 :   for (size_t i = 0; i < trajectory.size() - 1; i++) {
+    1420             : 
+    1421             :     // the trajectory sample
+    1422           0 :     const vec3_t sample = vec3_t(trajectory[i].position_W[0], trajectory[i].position_W[1], trajectory[i].position_W[2]);
+    1423             : 
+    1424             :     // next sample
+    1425           0 :     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]);
+    1426             : 
+    1427             :     // segment start
+    1428           0 :     const vec3_t segment_start =
+    1429           0 :         vec3_t(waypoints.at(last_segment_start).coords[0], waypoints.at(last_segment_start).coords[1], waypoints.at(last_segment_start).coords[2]);
+    1430             : 
+    1431             :     // segment end
+    1432           0 :     const vec3_t segment_end =
+    1433           0 :         vec3_t(waypoints.at(last_segment_start + 1).coords[0], waypoints.at(last_segment_start + 1).coords[1], waypoints.at(last_segment_start + 1).coords[2]);
+    1434             : 
+    1435           0 :     const double distance_from_segment = distFromSegment(sample, segment_start, segment_end);
+    1436             : 
+    1437           0 :     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
+    1438             : 
+    1439           0 :     if (segment_end_dist < 0.05 && last_segment_start < (int(waypoints.size()) - 2)) {
+    1440           0 :       last_segment_start++;
+    1441             : 
+    1442           0 :       segment_centers.push_back(int(floor(double(i - last_segment_start_sample) / 2.0)));
+    1443             :     }
+    1444             :   }
+    1445             : 
+    1446           0 :   return segment_centers;
+    1447             : }
+    1448             : 
+    1449             : //}
+    1450             : 
+    1451             : // | --------------------- minor routines --------------------- |
+    1452             : 
+    1453             : /* findTrajectoryAsync() //{ */
+    1454             : 
+    1455           1 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryAsync(const std::vector<Waypoint_t>&  waypoints,
+    1456             :                                                                                                        const mrs_msgs::TrackerCommand& initial_state,
+    1457             :                                                                                                        const double& sampling_dt, const bool& relax_heading) {
+    1458             : 
+    1459           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: starting the async planning task");
+    1460             : 
+    1461           1 :   future_trajectory_result_ =
+    1462           1 :       std::async(std::launch::async, &MrsTrajectoryGeneration::findTrajectory, this, waypoints, initial_state, sampling_dt, relax_heading);
+    1463             : 
+    1464           1 :   while (ros::ok() && future_trajectory_result_.wait_for(std::chrono::milliseconds(1)) != std::future_status::ready) {
+    1465             : 
+    1466           0 :     if (overtime()) {
+    1467           0 :       ROS_WARN("[MrsTrajectoryGeneration]: async task planning timeout, breaking");
+    1468           1 :       return {};
+    1469             :     }
+    1470             :   }
+    1471             : 
+    1472           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: async planning task finished successfully");
+    1473             : 
+    1474           1 :   return future_trajectory_result_.get();
+    1475             : }
+    1476             : 
+    1477             : //}
+    1478             : 
+    1479             : /* distFromSegment() //{ */
+    1480             : 
+    1481          78 : double MrsTrajectoryGeneration::distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2) {
+    1482             : 
+    1483          78 :   vec3_t segment_vector = seg2 - seg1;
+    1484          78 :   double segment_len    = segment_vector.norm();
+    1485             : 
+    1486          78 :   vec3_t segment_vector_norm = segment_vector;
+    1487          78 :   segment_vector_norm.normalize();
+    1488             : 
+    1489          78 :   double point_coordinate = segment_vector_norm.dot(point - seg1);
+    1490             : 
+    1491          78 :   if (point_coordinate < 0) {
+    1492           0 :     return (point - seg1).norm();
+    1493          78 :   } else if (point_coordinate > segment_len) {
+    1494          78 :     return (point - seg2).norm();
+    1495             :   } else {
+    1496             : 
+    1497          78 :     mat3_t segment_projector = segment_vector_norm * segment_vector_norm.transpose();
+    1498          39 :     vec3_t projection        = seg1 + segment_projector * (point - seg1);
+    1499             : 
+    1500          78 :     return (point - projection).norm();
+    1501             :   }
+    1502             : }
+    1503             : 
+    1504             : //}
+    1505             : 
+    1506             : /* getTrajectoryReference() //{ */
+    1507             : 
+    1508           1 : mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+    1509             :                                                                               const ros::Time& stamp, const double& sampling_dt) {
+    1510             : 
+    1511           1 :   mrs_msgs::TrajectoryReference msg;
+    1512             : 
+    1513           1 :   msg.header.frame_id = frame_id_;
+    1514           1 :   msg.header.stamp    = stamp;
+    1515           1 :   msg.fly_now         = fly_now_;
+    1516           1 :   msg.loop            = loop_;
+    1517           1 :   msg.use_heading     = use_heading_;
+    1518           1 :   msg.dt              = sampling_dt;
+    1519             : 
+    1520          41 :   for (size_t it = 0; it < trajectory.size(); it++) {
+    1521             : 
+    1522          40 :     mrs_msgs::Reference point;
+    1523          40 :     point.heading    = 0;
+    1524          40 :     point.position.x = trajectory[it].position_W[0];
+    1525          40 :     point.position.y = trajectory[it].position_W[1];
+    1526          40 :     point.position.z = trajectory[it].position_W[2];
+    1527          40 :     point.heading    = trajectory[it].getYaw();
+    1528             : 
+    1529          40 :     msg.points.push_back(point);
+    1530             :   }
+    1531             : 
+    1532           1 :   return msg;
+    1533             : }
+    1534             : 
+    1535             : //}
+    1536             : 
+    1537             : /* interpolatePoint() //{ */
+    1538             : 
+    1539           0 : Waypoint_t MrsTrajectoryGeneration::interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff) {
+    1540             : 
+    1541           0 :   Waypoint_t      out;
+    1542           0 :   Eigen::Vector4d diff = b.coords - a.coords;
+    1543             : 
+    1544           0 :   out.coords[0] = a.coords[0] + coeff * diff[0];
+    1545           0 :   out.coords[1] = a.coords[1] + coeff * diff[1];
+    1546           0 :   out.coords[2] = a.coords[2] + coeff * diff[2];
+    1547           0 :   out.coords[3] = radians::interp(a.coords[3], b.coords[3], coeff);
+    1548             : 
+    1549           0 :   out.stop_at = false;
+    1550             : 
+    1551           0 :   return out;
+    1552             : }
+    1553             : 
+    1554             : //}
+    1555             : 
+    1556             : /* checkNaN() //{ */
+    1557             : 
+    1558           1 : bool MrsTrajectoryGeneration::checkNaN(const Waypoint_t& a) {
+    1559             : 
+    1560           1 :   if (!std::isfinite(a.coords[0])) {
+    1561           0 :     ROS_ERROR("NaN detected in variable \"a.coords[0]\"!!!");
+    1562           0 :     return false;
+    1563             :   }
+    1564             : 
+    1565           1 :   if (!std::isfinite(a.coords[1])) {
+    1566           0 :     ROS_ERROR("NaN detected in variable \"a.coords[1]\"!!!");
+    1567           0 :     return false;
+    1568             :   }
+    1569             : 
+    1570           1 :   if (!std::isfinite(a.coords[2])) {
+    1571           0 :     ROS_ERROR("NaN detected in variable \"a.coords[2]\"!!!");
+    1572           0 :     return false;
+    1573             :   }
+    1574             : 
+    1575           1 :   if (!std::isfinite(a.coords[3])) {
+    1576           0 :     ROS_ERROR("NaN detected in variable \"a.coords[3]\"!!!");
+    1577           0 :     return false;
+    1578             :   }
+    1579             : 
+    1580             :   return true;
+    1581             : }
+    1582             : 
+    1583             : //}
+    1584             : 
+    1585             : /* trajectorySrv() //{ */
+    1586             : 
+    1587           1 : bool MrsTrajectoryGeneration::trajectorySrv(const mrs_msgs::TrajectoryReference& msg) {
+    1588             : 
+    1589           2 :   mrs_msgs::TrajectoryReferenceSrv srv;
+    1590           1 :   srv.request.trajectory = msg;
+    1591             : 
+    1592           1 :   bool res = service_client_trajectory_reference_.call(srv);
+    1593             : 
+    1594           1 :   if (res) {
+    1595             : 
+    1596           1 :     if (!srv.response.success) {
+    1597           0 :       ROS_WARN("[MrsTrajectoryGeneration]: service call for trajectory_reference returned: '%s'", srv.response.message.c_str());
+    1598             :     }
+    1599             : 
+    1600           1 :     return srv.response.success;
+    1601             : 
+    1602             :   } else {
+    1603             : 
+    1604           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: service call for trajectory_reference failed!");
+    1605             : 
+    1606           0 :     return false;
+    1607             :   }
+    1608             : }
+    1609             : 
+    1610             : //}
+    1611             : 
+    1612             : /* transformTrackerCmd() //{ */
+    1613             : 
+    1614           1 : std::optional<mrs_msgs::Path> MrsTrajectoryGeneration::transformPath(const mrs_msgs::Path& path_in, const std::string& target_frame) {
+    1615             : 
+    1616             :   // if we transform to the current control frame, which is in fact the same frame as the tracker_cmd is in
+    1617           1 :   if (target_frame == path_in.header.frame_id) {
+    1618           1 :     return path_in;
+    1619             :   }
+    1620             : 
+    1621             :   // find the transformation
+    1622           1 :   auto tf = transformer_->getTransform(path_in.header.frame_id, target_frame, path_in.header.stamp);
+    1623             : 
+    1624           0 :   if (!tf) {
+    1625           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: could not find transform from '%s' to '%s' in time %f", path_in.header.frame_id.c_str(), target_frame.c_str(),
+    1626             :               path_in.header.stamp.toSec());
+    1627           0 :     return {};
+    1628             :   }
+    1629             : 
+    1630           0 :   mrs_msgs::Path path_out = path_in;
+    1631             : 
+    1632           0 :   path_out.header.stamp    = tf.value().header.stamp;
+    1633           0 :   path_out.header.frame_id = transformer_->frame_to(tf.value());
+    1634             : 
+    1635           0 :   for (size_t i = 0; i < path_in.points.size(); i++) {
+    1636             : 
+    1637           0 :     mrs_msgs::ReferenceStamped waypoint;
+    1638             : 
+    1639           0 :     waypoint.header    = path_in.header;
+    1640           0 :     waypoint.reference = path_in.points[i];
+    1641             : 
+    1642           0 :     if (auto ret = transformer_->transform(waypoint, tf.value())) {
+    1643             : 
+    1644           0 :       path_out.points[i] = ret.value().reference;
+    1645             : 
+    1646             :     } else {
+    1647           0 :       return {};
+    1648             :     }
+    1649             :   }
+    1650             : 
+    1651             :   //}
+    1652             : 
+    1653           0 :   return path_out;
+    1654             : }
+    1655             : 
+    1656             : //}
+    1657             : 
+    1658             : /* overtime() //{ */
+    1659             : 
+    1660           4 : bool MrsTrajectoryGeneration::overtime(void) {
+    1661             : 
+    1662           4 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1663           4 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1664             : 
+    1665           4 :   double overtime = (ros::Time::now() - start_time_total).toSec();
+    1666             : 
+    1667           4 :   if (overtime > (OVERTIME_SAFETY_FACTOR * max_execution_time - OVERTIME_SAFETY_OFFSET)) {
+    1668           0 :     return true;
+    1669             :   }
+    1670             : 
+    1671             :   return false;
+    1672             : }
+    1673             : 
+    1674             : //}
+    1675             : 
+    1676             : /* timeLeft() //{ */
+    1677             : 
+    1678           1 : double MrsTrajectoryGeneration::timeLeft(void) {
+    1679             : 
+    1680           1 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1681           1 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1682             : 
+    1683           1 :   double current_execution_time = (ros::Time::now() - start_time_total).toSec();
+    1684             : 
+    1685           1 :   if (current_execution_time >= max_execution_time) {
+    1686             :     return 0;
+    1687             :   } else {
+    1688           1 :     return max_execution_time - current_execution_time;
+    1689             :   }
+    1690             : }
+    1691             : 
+    1692             : //}
+    1693             : 
+    1694             : // | ------------------------ callbacks ----------------------- |
+    1695             : 
+    1696             : /* callbackPath() //{ */
+    1697             : 
+    1698           0 : void MrsTrajectoryGeneration::callbackPath(const mrs_msgs::PathConstPtr& msg) {
+    1699             : 
+    1700           0 :   if (!is_initialized_) {
+    1701           0 :     return;
+    1702             :   }
+    1703             : 
+    1704             :   /* preconditions //{ */
+    1705             : 
+    1706           0 :   if (!got_constraints_) {
+    1707           0 :     std::stringstream ss;
+    1708           0 :     ss << "missing constraints";
+    1709           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1710           0 :     return;
+    1711             :   }
+    1712             : 
+    1713           0 :   if (!got_tracker_cmd_) {
+    1714           0 :     std::stringstream ss;
+    1715           0 :     ss << "missing position cmd";
+    1716           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1717           0 :     return;
+    1718             :   }
+    1719             : 
+    1720           0 :   if (!got_control_manager_diag_) {
+    1721           0 :     std::stringstream ss;
+    1722           0 :     ss << "missing control manager diagnostics";
+    1723           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1724           0 :     return;
+    1725             :   }
+    1726             : 
+    1727             :   //}
+    1728             : 
+    1729           0 :   {
+    1730           0 :     std::scoped_lock lock(mutex_start_time_total_);
+    1731             : 
+    1732           0 :     start_time_total_ = ros::Time::now();
+    1733             :   }
+    1734             : 
+    1735           0 :   double path_time_offset = 0;
+    1736             : 
+    1737           0 :   if (msg->header.stamp != ros::Time(0)) {
+    1738           0 :     path_time_offset = (msg->header.stamp - ros::Time::now()).toSec();
+    1739             :   }
+    1740             : 
+    1741           0 :   if (path_time_offset > 1e-3) {
+    1742             : 
+    1743           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1744             : 
+    1745           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time);
+    1746             : 
+    1747           0 :     ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1748             :              path_time_offset);
+    1749             :   } else {
+    1750             : 
+    1751           0 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1752             : 
+    1753           0 :     max_execution_time_ = params_.max_time;
+    1754             :   }
+    1755             : 
+    1756           0 :   ROS_INFO("[MrsTrajectoryGeneration]: got path from message");
+    1757             : 
+    1758           0 :   ph_original_path_.publish(msg);
+    1759             : 
+    1760           0 :   if (msg->points.empty()) {
+    1761           0 :     std::stringstream ss;
+    1762           0 :     ss << "received an empty message";
+    1763           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1764           0 :     return;
+    1765             :   }
+    1766             : 
+    1767           0 :   auto transformed_path = transformPath(*msg, "");
+    1768             : 
+    1769           0 :   if (!transformed_path) {
+    1770           0 :     std::stringstream ss;
+    1771           0 :     ss << "could not transform the path to the current control frame";
+    1772           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1773           0 :     return;
+    1774             :   }
+    1775             : 
+    1776           0 :   fly_now_                              = transformed_path->fly_now;
+    1777           0 :   use_heading_                          = transformed_path->use_heading;
+    1778           0 :   frame_id_                             = transformed_path->header.frame_id;
+    1779           0 :   override_constraints_                 = transformed_path->override_constraints;
+    1780           0 :   loop_                                 = transformed_path->loop;
+    1781           0 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1782           0 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1783           0 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1784           0 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1785           0 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1786           0 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1787           0 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1788             : 
+    1789           0 :   std::vector<Waypoint_t> waypoints;
+    1790             : 
+    1791           0 :   for (size_t i = 0; i < transformed_path->points.size(); i++) {
+    1792             : 
+    1793           0 :     double x       = transformed_path->points[i].position.x;
+    1794           0 :     double y       = transformed_path->points[i].position.y;
+    1795           0 :     double z       = transformed_path->points[i].position.z;
+    1796           0 :     double heading = transformed_path->points[i].heading;
+    1797             : 
+    1798           0 :     Waypoint_t wp;
+    1799           0 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    1800           0 :     wp.stop_at = stop_at_waypoints_;
+    1801             : 
+    1802           0 :     if (!checkNaN(wp)) {
+    1803           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    1804           0 :       return;
+    1805             :     }
+    1806             : 
+    1807           0 :     waypoints.push_back(wp);
+    1808             :   }
+    1809             : 
+    1810           0 :   if (loop_) {
+    1811           0 :     waypoints.push_back(waypoints[0]);
+    1812             :   }
+    1813             : 
+    1814           0 :   bool                          success = false;
+    1815           0 :   std::string                   message;
+    1816           0 :   mrs_msgs::TrajectoryReference trajectory;
+    1817             : 
+    1818           0 :   for (int i = 0; i < _n_attempts_; i++) {
+    1819             : 
+    1820           0 :     auto tracker_cmd = mrs_lib::get_mutexed(mutex_tracker_cmd_, tracker_cmd_);
+    1821             : 
+    1822             :     // the last iteration and the fallback sampling is enabled
+    1823           0 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    1824             : 
+    1825           0 :     std::tie(success, message, trajectory) =
+    1826           0 :         optimize(waypoints, transformed_path->header, tracker_cmd, tracker_cmd.full_state_prediction, fallback_sampling, transformed_path->relax_heading);
+    1827             : 
+    1828           0 :     if (success) {
+    1829             :       break;
+    1830             :     } else {
+    1831           0 :       if (i < _n_attempts_) {
+    1832           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    1833             :       } else {
+    1834           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    1835             :       }
+    1836             :     }
+    1837             :   }
+    1838             : 
+    1839           0 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    1840             : 
+    1841           0 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1842             : 
+    1843           0 :   if (total_time > max_execution_time) {
+    1844           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    1845             :               total_time - max_execution_time);
+    1846             :   } else {
+    1847           0 :     ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    1848             :   }
+    1849             : 
+    1850           0 :   trajectory.input_id = transformed_path->input_id;
+    1851             : 
+    1852           0 :   if (success) {
+    1853             : 
+    1854           0 :     bool published = trajectorySrv(trajectory);
+    1855             : 
+    1856           0 :     if (published) {
+    1857             : 
+    1858           0 :       ROS_INFO("[MrsTrajectoryGeneration]: trajectory successfully published");
+    1859             : 
+    1860             :     } else {
+    1861             : 
+    1862           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: could not publish the trajectory");
+    1863             :     }
+    1864             : 
+    1865             :   } else {
+    1866             : 
+    1867           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, no publishing a result");
+    1868             :   }
+    1869             : }
+    1870             : 
+    1871             : //}
+    1872             : 
+    1873             : /* callbackPathSrv() //{ */
+    1874             : 
+    1875           1 : bool MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res) {
+    1876             : 
+    1877           1 :   if (!is_initialized_) {
+    1878             :     return false;
+    1879             :   }
+    1880             : 
+    1881             :   /* mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("callback()"); */
+    1882             : 
+    1883             :   /* precondition //{ */
+    1884             : 
+    1885           1 :   if (!got_constraints_) {
+    1886           0 :     std::stringstream ss;
+    1887           0 :     ss << "missing constraints";
+    1888           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1889             : 
+    1890           0 :     res.message = ss.str();
+    1891           0 :     res.success = false;
+    1892           0 :     return true;
+    1893             :   }
+    1894             : 
+    1895           1 :   if (!got_tracker_cmd_) {
+    1896           0 :     std::stringstream ss;
+    1897           0 :     ss << "missing position cmd";
+    1898           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1899             : 
+    1900           0 :     res.message = ss.str();
+    1901           0 :     res.success = false;
+    1902           0 :     return true;
+    1903             :   }
+    1904             : 
+    1905           1 :   if (!got_control_manager_diag_) {
+    1906           0 :     std::stringstream ss;
+    1907           0 :     ss << "missing control manager diagnostics";
+    1908           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1909             : 
+    1910           0 :     res.message = ss.str();
+    1911           0 :     res.success = false;
+    1912           0 :     return true;
+    1913             :   }
+    1914             : 
+    1915             :   //}
+    1916             : 
+    1917           1 :   {
+    1918           1 :     std::scoped_lock lock(mutex_start_time_total_);
+    1919             : 
+    1920           1 :     start_time_total_ = ros::Time::now();
+    1921             :   }
+    1922             : 
+    1923           1 :   double path_time_offset = 0;
+    1924             : 
+    1925           1 :   if (req.path.header.stamp != ros::Time(0)) {
+    1926           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    1927             :   }
+    1928             : 
+    1929           1 :   if (path_time_offset > 1e-3) {
+    1930             : 
+    1931           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1932             : 
+    1933           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time);
+    1934             : 
+    1935           0 :     ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1936             :              path_time_offset);
+    1937             :   } else {
+    1938             : 
+    1939           2 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1940             : 
+    1941           1 :     max_execution_time_ = params_.max_time;
+    1942             :   }
+    1943             : 
+    1944           2 :   ROS_INFO("[MrsTrajectoryGeneration]: got path from service");
+    1945             : 
+    1946           1 :   ph_original_path_.publish(req.path);
+    1947             : 
+    1948           1 :   if (req.path.points.empty()) {
+    1949           0 :     std::stringstream ss;
+    1950           0 :     ss << "received an empty message";
+    1951           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1952             : 
+    1953           0 :     res.message = ss.str();
+    1954           0 :     res.success = false;
+    1955           0 :     return true;
+    1956             :   }
+    1957             : 
+    1958           2 :   auto transformed_path = transformPath(req.path, "");
+    1959             : 
+    1960           1 :   if (!transformed_path) {
+    1961           0 :     std::stringstream ss;
+    1962           0 :     ss << "could not transform the path to the current control frame";
+    1963           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1964             : 
+    1965           0 :     res.message = ss.str();
+    1966           0 :     res.success = false;
+    1967           0 :     return true;
+    1968             :   }
+    1969             : 
+    1970           1 :   fly_now_                              = transformed_path->fly_now;
+    1971           1 :   use_heading_                          = transformed_path->use_heading;
+    1972           1 :   frame_id_                             = transformed_path->header.frame_id;
+    1973           1 :   override_constraints_                 = transformed_path->override_constraints;
+    1974           1 :   loop_                                 = transformed_path->loop;
+    1975           1 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1976           1 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1977           1 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1978           1 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1979           1 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1980           1 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1981           1 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1982             : 
+    1983           2 :   std::vector<Waypoint_t> waypoints;
+    1984             : 
+    1985           2 :   for (size_t i = 0; i < req.path.points.size(); i++) {
+    1986             : 
+    1987           1 :     double x       = transformed_path->points[i].position.x;
+    1988           1 :     double y       = transformed_path->points[i].position.y;
+    1989           1 :     double z       = transformed_path->points[i].position.z;
+    1990           1 :     double heading = transformed_path->points[i].heading;
+    1991             : 
+    1992           1 :     Waypoint_t wp;
+    1993           1 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    1994           1 :     wp.stop_at = stop_at_waypoints_;
+    1995             : 
+    1996           1 :     if (!checkNaN(wp)) {
+    1997           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    1998           0 :       res.success = false;
+    1999           0 :       res.message = "invalid path";
+    2000           0 :       return true;
+    2001             :     }
+    2002             : 
+    2003           1 :     waypoints.push_back(wp);
+    2004             :   }
+    2005             : 
+    2006           1 :   if (loop_) {
+    2007           0 :     waypoints.push_back(waypoints[0]);
+    2008             :   }
+    2009             : 
+    2010           1 :   bool                          success = false;
+    2011           2 :   std::string                   message;
+    2012           2 :   mrs_msgs::TrajectoryReference trajectory;
+    2013             : 
+    2014           1 :   for (int i = 0; i < _n_attempts_; i++) {
+    2015             : 
+    2016           1 :     auto tracker_cmd = mrs_lib::get_mutexed(mutex_tracker_cmd_, tracker_cmd_);
+    2017             : 
+    2018             :     // the last iteration and the fallback sampling is enabled
+    2019           1 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2020             : 
+    2021           1 :     std::tie(success, message, trajectory) =
+    2022           2 :         optimize(waypoints, transformed_path->header, tracker_cmd, tracker_cmd.full_state_prediction, fallback_sampling, transformed_path->relax_heading);
+    2023             : 
+    2024           1 :     if (success) {
+    2025             :       break;
+    2026             :     } else {
+    2027           0 :       if (i < _n_attempts_) {
+    2028           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2029             :       } else {
+    2030           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2031             :       }
+    2032             :     }
+    2033             :   }
+    2034             : 
+    2035           1 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2036             : 
+    2037           1 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2038             : 
+    2039           1 :   if (total_time > max_execution_time) {
+    2040           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2041             :               total_time - max_execution_time);
+    2042             :   } else {
+    2043           2 :     ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2044             :   }
+    2045             : 
+    2046           1 :   trajectory.input_id = transformed_path->input_id;
+    2047             : 
+    2048           1 :   if (success) {
+    2049             : 
+    2050           1 :     bool published = trajectorySrv(trajectory);
+    2051             : 
+    2052           1 :     if (published) {
+    2053             : 
+    2054           1 :       res.success = success;
+    2055           1 :       res.message = message;
+    2056             : 
+    2057             :     } else {
+    2058             : 
+    2059           0 :       std::stringstream ss;
+    2060           0 :       ss << "could not publish the trajectory";
+    2061             : 
+    2062           0 :       res.success = false;
+    2063           0 :       res.message = ss.str();
+    2064             : 
+    2065           0 :       ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+    2066             :     }
+    2067             : 
+    2068             :   } else {
+    2069             : 
+    2070           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, not publishing a result");
+    2071             : 
+    2072           0 :     res.success = success;
+    2073           0 :     res.message = message;
+    2074             :   }
+    2075             : 
+    2076           1 :   return true;
+    2077             : }
+    2078             : 
+    2079             : //}
+    2080             : 
+    2081             : /* callbackGetPathSrv() //{ */
+    2082             : 
+    2083           0 : bool MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res) {
+    2084             : 
+    2085           0 :   if (!is_initialized_) {
+    2086             :     return false;
+    2087             :   }
+    2088             : 
+    2089             :   /* precondition //{ */
+    2090             : 
+    2091           0 :   if (!got_constraints_) {
+    2092           0 :     std::stringstream ss;
+    2093           0 :     ss << "missing constraints";
+    2094           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2095             : 
+    2096           0 :     res.message = ss.str();
+    2097           0 :     res.success = false;
+    2098           0 :     return true;
+    2099             :   }
+    2100             : 
+    2101           0 :   if (!got_tracker_cmd_) {
+    2102           0 :     std::stringstream ss;
+    2103           0 :     ss << "missing position cmd";
+    2104           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2105             : 
+    2106           0 :     res.message = ss.str();
+    2107           0 :     res.success = false;
+    2108           0 :     return true;
+    2109             :   }
+    2110             : 
+    2111           0 :   if (!got_control_manager_diag_) {
+    2112           0 :     std::stringstream ss;
+    2113           0 :     ss << "missing control manager diagnostics";
+    2114           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2115             : 
+    2116           0 :     res.message = ss.str();
+    2117           0 :     res.success = false;
+    2118           0 :     return true;
+    2119             :   }
+    2120             : 
+    2121             :   //}
+    2122             : 
+    2123           0 :   {
+    2124           0 :     std::scoped_lock lock(mutex_start_time_total_);
+    2125             : 
+    2126           0 :     start_time_total_ = ros::Time::now();
+    2127             :   }
+    2128             : 
+    2129           0 :   double path_time_offset = 0;
+    2130             : 
+    2131           0 :   if (req.path.header.stamp != ros::Time(0)) {
+    2132           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    2133             :   }
+    2134             : 
+    2135           0 :   if (path_time_offset > 1e-3) {
+    2136             : 
+    2137           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2138             : 
+    2139           0 :     max_execution_time_ = FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset;
+    2140             : 
+    2141           0 :     ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    2142             :              path_time_offset);
+    2143             :   } else {
+    2144             : 
+    2145           0 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    2146             : 
+    2147           0 :     max_execution_time_ = params_.max_time;
+    2148             :   }
+    2149             : 
+    2150           0 :   ROS_INFO("[MrsTrajectoryGeneration]: got path from service");
+    2151             : 
+    2152           0 :   ph_original_path_.publish(req.path);
+    2153             : 
+    2154           0 :   if (req.path.points.empty()) {
+    2155           0 :     std::stringstream ss;
+    2156           0 :     ss << "received an empty message";
+    2157           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2158             : 
+    2159           0 :     res.message = ss.str();
+    2160           0 :     res.success = false;
+    2161           0 :     return true;
+    2162             :   }
+    2163             : 
+    2164           0 :   fly_now_                              = req.path.fly_now;
+    2165           0 :   use_heading_                          = req.path.use_heading;
+    2166           0 :   frame_id_                             = req.path.header.frame_id;
+    2167           0 :   override_constraints_                 = req.path.override_constraints;
+    2168           0 :   loop_                                 = req.path.loop;
+    2169           0 :   override_max_velocity_horizontal_     = req.path.override_max_velocity_horizontal;
+    2170           0 :   override_max_velocity_vertical_       = req.path.override_max_velocity_vertical;
+    2171           0 :   override_max_acceleration_horizontal_ = req.path.override_max_acceleration_horizontal;
+    2172           0 :   override_max_acceleration_vertical_   = req.path.override_max_acceleration_vertical;
+    2173           0 :   override_max_jerk_horizontal_         = req.path.override_max_jerk_horizontal;
+    2174           0 :   override_max_jerk_vertical_           = req.path.override_max_jerk_horizontal;
+    2175           0 :   stop_at_waypoints_                    = req.path.stop_at_waypoints;
+    2176             : 
+    2177           0 :   std::vector<Waypoint_t> waypoints;
+    2178             : 
+    2179           0 :   for (size_t i = 0; i < req.path.points.size(); i++) {
+    2180             : 
+    2181           0 :     double x       = req.path.points[i].position.x;
+    2182           0 :     double y       = req.path.points[i].position.y;
+    2183           0 :     double z       = req.path.points[i].position.z;
+    2184           0 :     double heading = req.path.points[i].heading;
+    2185             : 
+    2186           0 :     Waypoint_t wp;
+    2187           0 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    2188           0 :     wp.stop_at = stop_at_waypoints_;
+    2189             : 
+    2190           0 :     if (!checkNaN(wp)) {
+    2191           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    2192           0 :       res.success = false;
+    2193           0 :       res.message = "invalid path";
+    2194           0 :       return true;
+    2195             :     }
+    2196             : 
+    2197           0 :     waypoints.push_back(wp);
+    2198             :   }
+    2199             : 
+    2200           0 :   if (loop_) {
+    2201           0 :     waypoints.push_back(waypoints[0]);
+    2202             :   }
+    2203             : 
+    2204           0 :   bool                          success = false;
+    2205           0 :   std::string                   message;
+    2206           0 :   mrs_msgs::TrajectoryReference trajectory;
+    2207             : 
+    2208           0 :   for (int i = 0; i < _n_attempts_; i++) {
+    2209             : 
+    2210           0 :     auto tracker_cmd = mrs_lib::get_mutexed(mutex_tracker_cmd_, tracker_cmd_);
+    2211             : 
+    2212             :     // the last iteration and the fallback sampling is enabled
+    2213           0 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2214             : 
+    2215           0 :     std::tie(success, message, trajectory) =
+    2216           0 :         optimize(waypoints, req.path.header, tracker_cmd, tracker_cmd.full_state_prediction, fallback_sampling, req.path.relax_heading);
+    2217             : 
+    2218           0 :     if (success) {
+    2219             :       break;
+    2220             :     } else {
+    2221           0 :       if (i < _n_attempts_) {
+    2222           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2223             :       } else {
+    2224           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2225             :       }
+    2226             :     }
+    2227             :   }
+    2228             : 
+    2229           0 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2230             : 
+    2231           0 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2232             : 
+    2233           0 :   if (total_time > max_execution_time) {
+    2234           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2235             :               total_time - max_execution_time);
+    2236             :   } else {
+    2237           0 :     ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2238             :   }
+    2239             : 
+    2240           0 :   if (success) {
+    2241             : 
+    2242           0 :     res.trajectory = trajectory;
+    2243           0 :     res.success    = success;
+    2244           0 :     res.message    = message;
+    2245             : 
+    2246             :   } else {
+    2247             : 
+    2248           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2249             : 
+    2250           0 :     res.success = success;
+    2251           0 :     res.message = message;
+    2252             :   }
+    2253             : 
+    2254           0 :   return true;
+    2255             : }
+    2256             : 
+    2257             : //}
+    2258             : 
+    2259             : /* callbackConstraints() //{ */
+    2260             : 
+    2261        3831 : void MrsTrajectoryGeneration::callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg) {
+    2262             : 
+    2263        3831 :   if (!is_initialized_) {
+    2264             :     return;
+    2265             :   }
+    2266             : 
+    2267        3873 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got constraints");
+    2268             : 
+    2269        3831 :   mrs_lib::set_mutexed(mutex_constraints_, *msg, constraints_);
+    2270             : 
+    2271        3831 :   got_constraints_ = true;
+    2272             : }
+    2273             : 
+    2274             : //}
+    2275             : 
+    2276             : /* callbackTrackerCmd() //{ */
+    2277             : 
+    2278       31927 : void MrsTrajectoryGeneration::callbackTrackerCmd(const mrs_msgs::TrackerCommandConstPtr& msg) {
+    2279             : 
+    2280       31927 :   if (!is_initialized_) {
+    2281             :     return;
+    2282             :   }
+    2283             : 
+    2284       31966 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got position cmd");
+    2285             : 
+    2286       31927 :   got_tracker_cmd_ = true;
+    2287             : 
+    2288       31927 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    2289             : 
+    2290       31927 :   mrs_lib::set_mutexed(mutex_tracker_cmd_, *msg, tracker_cmd_);
+    2291             : }
+    2292             : 
+    2293             : //}
+    2294             : 
+    2295             : /* callbackControlManagerDiag() //{ */
+    2296             : 
+    2297        4897 : void MrsTrajectoryGeneration::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg) {
+    2298             : 
+    2299        4897 :   if (!is_initialized_) {
+    2300             :     return;
+    2301             :   }
+    2302             : 
+    2303        4939 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got control manager diagnostics");
+    2304             : 
+    2305        4897 :   got_control_manager_diag_ = true;
+    2306             : 
+    2307        4897 :   mrs_lib::set_mutexed(mutex_control_manager_diag_, *msg, control_manager_diag_);
+    2308             : }
+    2309             : 
+    2310             : //}
+    2311             : 
+    2312             : /* //{ callbackDrs() */
+    2313             : 
+    2314          42 : void MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, [[maybe_unused]] uint32_t level) {
+    2315             : 
+    2316          84 :   mrs_lib::set_mutexed(mutex_params_, params, params_);
+    2317             : 
+    2318          42 :   {
+    2319          42 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2320             : 
+    2321          42 :     max_execution_time_ = params.max_time;
+    2322             :   }
+    2323             : 
+    2324          84 :   ROS_INFO("[MrsTrajectoryGeneration]: DRS updated");
+    2325          42 : }
+    2326             : 
+    2327             : //}
+    2328             : 
+    2329             : }  // namespace mrs_uav_trajectory_generation
+    2330             : 
+    2331             : #include <pluginlib/class_list_macros.h>
+    2332             : 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..6841a81f7d --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html @@ -0,0 +1,603 @@ + + + + + + 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 + + +
+ 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..843648107f6a4a84cf129974ff37ac161702d557 GIT binary patch literal 7770 zcmV-g9;M-lP)x+>lvSs~uQSs~uQ*?*NATR%V`fh;vAz14fSO?xOm{SX3lxlOm*?fU-T zrB{37JCNRLC=*}>6arSU8hM0G33%;D`W&vG;jw@b0qQXpxTB;EJQ{@2^tH?+kMWGo z1&le@5@1HVh_MTgqPRI}Euadcg-4M~3aGU@so8Q+3KZKn;^>(nGqiZy(;g*g)cg?TH#qIpD#{wFWo^ zEtmjHJjxiYQr7|XijLZ=jhR5UHoTX|-j;i4!Kr7eb+w7K5>pzFeJJl3gWmQvvWsay;U2uvzG~nxFxhLw-IOMm9#BvE z+Eds}(nvl9C>-N1ytOByu1kFjL$y=qh&(c+RmZ)3wHKvsUH4A(wRFJl zd(!kvinHW=Cmk!W-!0mfl+f2TzyMDEftnf#kbsr|(LYrHlLjTgObt2U4PlIU>4w4r zUN1_(o7<5!fL9g9>qSTUdf@=oE$&=sq{Z#4ZJ1ra^qNE){}w4`z1`S)pTqISehmvH z_*LU9#65UYc*x@v{-4Z`CLWnzLaBplb74@IHi+a8hECa~zmGifd&3l0I)<;4;{x&M z5BgWmK&|i?*yMRU$7Ptx6)oaZT*kr#mGf7MVyq3dT_U#BO6FZYq1E3C{x#1v<`GcKL_Z+gEOTzKN%U7fag!cSkpQl(T<+J zSC@B6dcsJstD?nJ(EiC3jFQ8#=o#@o@+F> z5~}Hztp{5EOmhZL19r&iNkfcW130y^^aiZ+7!mdhw@{J>eUShQgUHBZ*Pa>Fc$P9# zsSV>T+v0Tg%><{Tl{Ac2@L!|Tf4W+?IK1X?)`YQP4x`E9SL*nwg~waAtQdE!h%xS5 zsT;sC_uN4o{XXmBe8@n-t|~zJ^g&2d9(mrgU4gO!0nx8>c@f5@6FanLuHM$YM*Z|~ zk;ogYjmNNON0Z|ekI4~K36xUZRUDE9zzS&e(eb zGoDKW>7VD((lZ7C20$(8{vP&wR7Co~PcC~GE}{>K8c4+>(1~5)nq1tyb_Fnc zns^Ktcioi4z9^4{pUudD`u3_mWpU~K;nQ>lgxxnE9*e0?F5LLIP6*2*%U1xrrJIKk z3*mkYe>DNyGDh!!;D{V!3N7NmGGW4up8Q1(3kZNcevU^v8?H&Jqwn;=59$^tT(>I& zZsq2tWXl@96kOe{=FCEu2Lx!r7>?Y_jPFzy!@ojrQk zxDn&ZGO%TUTLKK92u3sMAjTVEZb}<5u8Fl0pdRCPFt~a3?w{88_Y)Y|&L+QDAt3FU zPNV_a&Ha9B!5Fb@+l#c#|D(s4OHBXd0^p8+E0TT|01%+&e75gNowrS>owcgRSDb7` zK-F2ZzbwbHm+dmlUCB=@dlbU|Xq(p*-%2Lhwt=2uhOYA%9IJt!u6qL*MAF~T*T-g7 zh-Zj<;XVwYYs*m_>~5UAYW64zgVbJO|jgu!uLyQ7WvEQxGL2r{>8r&>0mSSkWbQfq*_>eZpV_ArOWP1Z*Q&3?yJ5 zu_wbf>bHetJ?>E?Uem91`2qn0`yxgi>BD2EPBBup;CqbUgEufGiA^^u^(u+eq({=b z9Wd@IM|O&XqsI=&oeLSjW3>_hTZQqzW3f^mwSclfCX9ArVq3v~eyM84m}d4FhX!Kr zeFOr2g&u(PTqXeSzlwlJoMvG>7C>I+kpTKX=DRRoQ8yDtvoIxZ*bb<(6%4Cm_XU>= zy*wPn(V=^|!7WyE78Wl7jyM-0CyLMvE@ZX+n0c=Oxdj7KjfcZ~hEb=VF(v%cy%e}G zi@!X3H(XM93*gU278GKJ5t%yRdD3D?wGPgnx|7+`OkczMvRMvI2NqY+ZFgABTR20c zO&V+*36O2;x*_Nb@z}p!j33ki4mj?CUICx!GSIY0y^+xTn$!iNdeBBVM(PXoc$C!> z1u97mWUS09#D(Q)%#v(PJ`*wsfIKm;u9 zA0Qhrb(aX3Fh0aFes)8eGvqDt6lL#z(|9b+R@sBJ?bdb-C_N5mZt!iD+sey+TrAxa zMnK;qix4~lg!#!lBsZd3l7VJ+9U&JUn86dBTIv4_WuunR*Z8Re(A%~Vw*`v`(tX(5KxKn zDdbALj*TtsaVI^RZm}hf$P94vzrbsPJ(dea9c>{%X(fPajPaU;MucU6YK%`Bj<*so z^17jbDlT|0MAdBo(0#&yVbX?+9K}pLCRkrjP86j=-Vzd{C!MWITvrwYfr7$c0I<4u z9m1f_6^2lOG&UAakhV*WjfGay^3JU!z1m6FM~{GUBbc{=4aSY{FsNGzjc|Tp)q?c zIGX~x_m0N#sBS41q#Vq~TYGt!h+Mt{8eoq>M*wErFrF7{u+jvosa^S+r78T8AH7Lx z6X|P8Wr~q5SIm#zm!c;PgKT^%;58JMgQe>5g0iKX_&g-${jcC6nno3G{y$qFP|7bW zgp!FtHe|K#5HJwU)K@rP8!+yF1Pt`;FdIn{ggFg?ZVB$o!QW|&aC14$V-{L>8lyv$ znHK_JZrL=EUWG8O^wECssaX~NJoBnVa)m4ng_)8t`tec$TwP{MngXjIV2dQ0{A`I1%ncd1*lndzsu(&gEfZEZ+M~BUGp4r5*UMyP% zWv>B7a;Rz=B`p;~TcTsAO=^dPQgDEbA>&CRKs{+L)GKdoR!YRn0^T2mAUptgL51qc zYwN`Qpavq|sG80S)CZwp3tah?})jeiB7R@<0UGoXBY5?IS z+FHbjc`1_K8ZSvgoJe`rHYaovCbfV<1}%Rbb%Mt{$5jg`2}1z8r7Syaj)ePq^tkED zw|BxT%j23Db>TNhDuo>$)&-vj{jJb0~Z@MeY;Upt-~WGM7jT`oNJUGb8g17h& zJW7Bn^;mBA(t)rTTZnlq1CHZSRwd-oXT5!jM^Tel zkM2)C!(%xQDe))-*!&2OGM8UBXXKv#TbCBq&*GWy@1e(?Ht5tjxk1Utti%%ulx#2ls4(l1GoWY5hI& z1%*pE2<2=&k11t zf|b~5T?>>vUvE$D>y8+M6VQF!Us-xDZRQ|tIiArI`E|oA-k8T^M4y`3*nCNLaDhEt z{ez7D_s1vb+>bGI(Zv@$i0^+A7W)m}#~mes$YLMXQFP2WzUpN$*Tw<09$0u*7{@JN z8%HrHlnqyZ0t$}5^ZoFr@J>NUPS*8ByUPkM-zTqty9pH+4Wp z-#rFy9^o4pld=`vuScwp>MnexZ45EhK4(-H=RiG-)k>xQSfHf9qeU2DrNDq(^{4KG zB8}TJlM%22*J3q)kTgJ*2K!b6JicyQ`uY+93EV0IuKvo_ecdc37hw%<^Bf>^MB2~? zjx<>hHcFb=ufYdV7{aiz3!&Y!uF?f9Qpn+0lK@?nxGkHnc$0*?YZ{V#?OB8ba6t#| zxOoi(4`0?-Dtf8|EOlYdWB2e-{*(eLpe2RtdGz*lr8XybcHOelfla1@zh*4}8Zd5g zl*^}f6GmTAvYPmn=vDyrq=TP+?RBVl61{|;Y{hudmil=HY4#yg;qe)I4=~CC>S`e$ zOa{ZJ!e(~|tv>zAUF9h}xIvaGX&qfdOYb<}@JG6G#2U8Oa7_tbgidd{r7^L^Tb^T3 zGE0gu46;GdJAv*=H$%o<7kJc>4#KR=Nj#1G6A!`0J%P)rgw&$OD;WC&iqrZ`=4S!W zfDr~4G92&Cx;~=uKS|rHb5|d=kKKn*44Ii-@Re#e4Vi(PzlIPcb5(yu8&}rj3v8}M zKnunf8%zeQ41`ezG?R`-{KkKO86QQLH4Z{m9Yzsy#TbE+1grta8wULFdmmU>$n$D` z)|xqhB1ilQP8>POj+7` zY$C?n#UJVRMW_fE#%?F#d=bVudr;B%Cdm{Qn-Xb16=rtWrr#JzSEy3b4-^Lt3sKrh zADv?&EyZA4Vy`1b!jXF!JbP z&1$6KJxY(TMnJl320+rUf-mlDWa^q7*(|P4r6mO}?U+H9eq$u9v83>QA#kR^=v+H# zjU|Oimuo-Bv8iBHXyDPP9vP#+W0q&sdXr1L2L%^5Vx(iuU-P#V1$E2;_a4H%;+u+I zZ%cE-A&cWPT!@YEo7GljM_TEeF*x13&9&T~KHg6|*LkN4Ehcnw3n!(`nzwt;G zW^Hy)(faBQ%?ZYZ3|^)fI$tK=L^?ehTxh21%HevgVF5_tOwp6fnpG$9@#A}y$Wi71~D49HElXFr(kULY(^3D9K0-(C#1Y_2tc{Bj%>*z07 zvK`)7%#uhg3I3NiTyA9W)2Rl#P}Q35kOFP7ua#X0Zl#UaQ67ig|?ZKSPuJM1Ne$H)Hb0wpE)jNi(#n#&@4qPl&!3`q|ew_o>id5{56ApeG$`W#ERd5!)gQkj9&NRBwh)<)ry!P(%M6DTYbZg!%I- zl*~wT{qSQmxeNP^dj<}Mqe>f7>B&EiwA+QV`#9ekgmI;M@w0z9_Gs-@N6)Y$X??JL z!E7Be!gWLZt44T+l%Q{Z!W>-Fa!FOiOCb+vtM<(+2$A`1cfF?Hat?F3%!t>?JRX$2igzPxXksKPUCb zI);9xJmwl5pYSC=tuV#Gg``$vlndt&jFG5vpV^%M>?K#S5xUs_^e^l)PqaKH4N#HV z6yDeFiL=2Z~+sBA}~go5>9UQ#CUBr<;tC z!s!5C!tTHTZYqphNoopL11u@rT823iCYUjx9wU4A5h$z)P>u00rJlmm7(W2`Vy}hB z8xR&@6!!%la9g3x$HhuUjEO@Lqn^irtauN}M?d(uhj=3LRo!yVd5i-}p|X9OlOALY zD~kOxYWz7KF{zgjeHtQDA)s(yNQVP6XuZ@*VX``ZLQOW?EX>m9DbJ>b)E5r829F_n z@f1c7plYo#*W490jX!}~dz~F z`*BzvL)2G_IknW#P`4io5A}v_+|GV$FlWEa`0}&z{zBYTBm&$Z7Xd>iat-5{mmOen z6I#Wr;ojC`?04l0;{Zw#>Yu%;R8FoZ$WE(N1WJH)jGu|`8Qe3SZb_9=|Jt4@EkK{W zpjAr!t9qvYW~h!q0(tCYU&{c^7(u)j3pgk}JrXbRs1*jquf+t59FV&k=5B|_&g;39 zuw^Pd#m>{eyJ1@sCs+L0JRY-RB>e0eG`TV0Np=2z@>!Ifx;zakpW$> zd$vMyA>H{Rwa6GLoDT3IgxfQKd;ot|^Qt5@g{uLU6#i&j>`P^|%!Q}$dmoQS_QHy_ z{M-G0XVd6^0Pw-Sq5-8bn+nxppB`|5wYz4nELp^uI21AJc?`&kbq1_A;`Id*0~Rvh zJz#w$9pD+pmA*=P9ANFuwN42=ll6jZfbIzQyN!@H;+eCki!8-8cK% zQyb&HXtDz1-yM6oje9Ld%q(2e=+O;|0XdNQ%Umqr2k+SPC$fTGAHL}J@ru+k)T_Yu z9MGdt;nl6HVa3G@?zs}4wU^7x81F!fod9*DhhD}PkL-RSeZAr-eHEsHW+jUl8|=p( z5B@y46~Ik$6hi#VLJ;)o0s8I(IK=ZI6}v}s=DX_S_PV&w?-STIEKyrN<$=B)7EjSH z`r$hgwRP=xxjw%mM_eiOukD%Aa`6}aP$B&V(GP!_@!d1yql183*FG8j@a38DXq5b} z=!d_|__8oLMoDTlMx^M{REWy^i+*4Le>O)PP^;yLZy2zYBfddE^4C(ZM*WLZ7>(W5%! ze$xX2eV^wMGrCdu$*7EO#l7#KI4MTn7%B*OR7=gYIlcOR0;Bj^ya)V$UY!>PF8YW6 z&%m9>4l_H(y!FJ$O|CDS3Z14yr7B1K$N$Y#2>%^Ju5p;{;eb*-fHsVnPqe*$%fJl} zEOf%HDJNqcxQwPBdGd|{yZ9c`Lx(T_>xiIR1tAx7`7`WFdd#2^4d2mT? g!TSZ92&ib&f8o0KqFv)t2mk;807*qoM6N<$f`{{`IRF3v 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