From aa00cb9bd60ee473b9f16d763556555b747b81f1 Mon Sep 17 00:00:00 2001 From: klaxalk Date: Sat, 8 Jun 2024 22:13:16 +0000 Subject: [PATCH] deploy: 4a913a5904aeb70a78a824e66b46252a6f9fcdf3 --- .nojekyll | 0 amber.png | Bin 0 -> 141 bytes badge.svg | 1 + emerald.png | Bin 0 -> 141 bytes gcov.css | 519 + glass.png | Bin 0 -> 167 bytes index-sort-f.html | 612 ++ index-sort-l.html | 612 ++ index.html | 612 ++ .../attitude_converter.h.func-sort-c.html | 124 + .../mrs_lib/attitude_converter.h.func.html | 124 + .../attitude_converter.h.gcov.frameset.html | 19 + .../mrs_lib/attitude_converter.h.gcov.html | 617 ++ .../attitude_converter.h.gcov.overview.html | 154 + .../mrs_lib/attitude_converter.h.gcov.png | Bin 0 -> 1675 bytes ...dynamic_reconfigure_mgr.h.func-sort-c.html | 420 + .../dynamic_reconfigure_mgr.h.func.html | 420 + ...namic_reconfigure_mgr.h.gcov.frameset.html | 19 + .../dynamic_reconfigure_mgr.h.gcov.html | 343 + ...namic_reconfigure_mgr.h.gcov.overview.html | 85 + .../dynamic_reconfigure_mgr.h.gcov.png | Bin 0 -> 1370 bytes .../geometry/cyclic.h.func-sort-c.html | 548 + .../mrs_lib/geometry/cyclic.h.func.html | 548 + .../geometry/cyclic.h.gcov.frameset.html | 19 + .../mrs_lib/geometry/cyclic.h.gcov.html | 612 ++ .../geometry/cyclic.h.gcov.overview.html | 152 + .../mrs_lib/geometry/cyclic.h.gcov.png | Bin 0 -> 2145 bytes .../mrs_lib/geometry/index-detail-sort-f.html | 128 + .../mrs_lib/geometry/index-detail-sort-l.html | 128 + .../mrs_lib/geometry/index-detail.html | 128 + .../mrs_lib/geometry/index-sort-f.html | 112 + .../mrs_lib/geometry/index-sort-l.html | 112 + mrs_lib/include/mrs_lib/geometry/index.html | 112 + .../mrs_lib/geometry/misc.h.func-sort-c.html | 92 + .../include/mrs_lib/geometry/misc.h.func.html | 92 + .../geometry/misc.h.gcov.frameset.html | 19 + .../include/mrs_lib/geometry/misc.h.gcov.html | 408 + .../geometry/misc.h.gcov.overview.html | 101 + .../include/mrs_lib/geometry/misc.h.gcov.png | Bin 0 -> 1071 bytes .../gps_conversions.h.func-sort-c.html | 96 + .../mrs_lib/gps_conversions.h.func.html | 96 + .../gps_conversions.h.gcov.frameset.html | 19 + .../mrs_lib/gps_conversions.h.gcov.html | 387 + .../gps_conversions.h.gcov.overview.html | 96 + .../mrs_lib/gps_conversions.h.gcov.png | Bin 0 -> 1579 bytes .../mrs_lib/impl/index-detail-sort-f.html | 236 + .../mrs_lib/impl/index-detail-sort-l.html | 236 + .../include/mrs_lib/impl/index-detail.html | 236 + .../include/mrs_lib/impl/index-sort-f.html | 172 + .../include/mrs_lib/impl/index-sort-l.html | 172 + mrs_lib/include/mrs_lib/impl/index.html | 172 + .../impl/param_provider.hpp.func-sort-c.html | 144 + .../mrs_lib/impl/param_provider.hpp.func.html | 144 + .../param_provider.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/param_provider.hpp.gcov.html | 132 + .../param_provider.hpp.gcov.overview.html | 32 + .../mrs_lib/impl/param_provider.hpp.gcov.png | Bin 0 -> 347 bytes .../publisher_handler.hpp.func-sort-c.html | 904 ++ .../impl/publisher_handler.hpp.func.html | 904 ++ .../publisher_handler.hpp.gcov.frameset.html | 19 + .../impl/publisher_handler.hpp.gcov.html | 332 + .../publisher_handler.hpp.gcov.overview.html | 82 + .../impl/publisher_handler.hpp.gcov.png | Bin 0 -> 798 bytes ...ervice_client_handler.hpp.func-sort-c.html | 268 + .../impl/service_client_handler.hpp.func.html | 268 + ...vice_client_handler.hpp.gcov.frameset.html | 19 + .../impl/service_client_handler.hpp.gcov.html | 403 + ...vice_client_handler.hpp.gcov.overview.html | 100 + .../impl/service_client_handler.hpp.gcov.png | Bin 0 -> 969 bytes .../subscribe_handler.hpp.func-sort-c.html | 4256 ++++++++ .../impl/subscribe_handler.hpp.func.html | 4256 ++++++++ .../subscribe_handler.hpp.gcov.frameset.html | 19 + .../impl/subscribe_handler.hpp.gcov.html | 446 + .../subscribe_handler.hpp.gcov.overview.html | 111 + .../impl/subscribe_handler.hpp.gcov.png | Bin 0 -> 1636 bytes .../mrs_lib/impl/timer.hpp.func-sort-c.html | 92 + .../include/mrs_lib/impl/timer.hpp.func.html | 92 + .../mrs_lib/impl/timer.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/timer.hpp.gcov.html | 180 + .../mrs_lib/impl/timer.hpp.gcov.overview.html | 44 + .../include/mrs_lib/impl/timer.hpp.gcov.png | Bin 0 -> 532 bytes .../impl/transformer.hpp.func-sort-c.html | 272 + .../mrs_lib/impl/transformer.hpp.func.html | 272 + .../impl/transformer.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/transformer.hpp.gcov.html | 280 + .../impl/transformer.hpp.gcov.overview.html | 69 + .../mrs_lib/impl/transformer.hpp.gcov.png | Bin 0 -> 841 bytes .../mrs_lib/impl/ukf.hpp.func-sort-c.html | 128 + .../include/mrs_lib/impl/ukf.hpp.func.html | 128 + .../mrs_lib/impl/ukf.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/ukf.hpp.gcov.html | 364 + .../mrs_lib/impl/ukf.hpp.gcov.overview.html | 90 + mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png | Bin 0 -> 1198 bytes .../vector_converter.hpp.func-sort-c.html | 152 + .../impl/vector_converter.hpp.func.html | 152 + .../vector_converter.hpp.gcov.frameset.html | 19 + .../impl/vector_converter.hpp.gcov.html | 173 + .../vector_converter.hpp.gcov.overview.html | 43 + .../impl/vector_converter.hpp.gcov.png | Bin 0 -> 456 bytes .../include/mrs_lib/index-detail-sort-f.html | 444 + .../include/mrs_lib/index-detail-sort-l.html | 444 + mrs_lib/include/mrs_lib/index-detail.html | 444 + mrs_lib/include/mrs_lib/index-sort-f.html | 292 + mrs_lib/include/mrs_lib/index-sort-l.html | 292 + mrs_lib/include/mrs_lib/index.html | 292 + .../include/mrs_lib/lkf.h.func-sort-c.html | 268 + mrs_lib/include/mrs_lib/lkf.h.func.html | 268 + .../include/mrs_lib/lkf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/lkf.h.gcov.html | 460 + .../include/mrs_lib/lkf.h.gcov.overview.html | 114 + mrs_lib/include/mrs_lib/lkf.h.gcov.png | Bin 0 -> 1750 bytes .../mrs_lib/msg_extractor.h.func-sort-c.html | 256 + .../include/mrs_lib/msg_extractor.h.func.html | 256 + .../msg_extractor.h.gcov.frameset.html | 19 + .../include/mrs_lib/msg_extractor.h.gcov.html | 775 ++ .../msg_extractor.h.gcov.overview.html | 193 + .../include/mrs_lib/msg_extractor.h.gcov.png | Bin 0 -> 1287 bytes .../include/mrs_lib/mutex.h.func-sort-c.html | 400 + mrs_lib/include/mrs_lib/mutex.h.func.html | 400 + .../mrs_lib/mutex.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/mutex.h.gcov.html | 260 + .../mrs_lib/mutex.h.gcov.overview.html | 64 + mrs_lib/include/mrs_lib/mutex.h.gcov.png | Bin 0 -> 739 bytes .../mrs_lib/param_loader.h.func-sort-c.html | 392 + .../include/mrs_lib/param_loader.h.func.html | 392 + .../mrs_lib/param_loader.h.gcov.frameset.html | 19 + .../include/mrs_lib/param_loader.h.gcov.html | 1452 +++ .../mrs_lib/param_loader.h.gcov.overview.html | 362 + .../include/mrs_lib/param_loader.h.gcov.png | Bin 0 -> 4406 bytes .../publisher_handler.h.func-sort-c.html | 556 + .../mrs_lib/publisher_handler.h.func.html | 556 + .../publisher_handler.h.gcov.frameset.html | 19 + .../mrs_lib/publisher_handler.h.gcov.html | 256 + .../publisher_handler.h.gcov.overview.html | 63 + .../mrs_lib/publisher_handler.h.gcov.png | Bin 0 -> 572 bytes ...uadratic_throttle_model.h.func-sort-c.html | 88 + .../quadratic_throttle_model.h.func.html | 88 + ...dratic_throttle_model.h.gcov.frameset.html | 19 + .../quadratic_throttle_model.h.gcov.html | 117 + ...dratic_throttle_model.h.gcov.overview.html | 29 + .../quadratic_throttle_model.h.gcov.png | Bin 0 -> 246 bytes .../mrs_lib/repredictor.h.func-sort-c.html | 256 + .../include/mrs_lib/repredictor.h.func.html | 256 + .../mrs_lib/repredictor.h.gcov.frameset.html | 19 + .../include/mrs_lib/repredictor.h.gcov.html | 637 ++ .../mrs_lib/repredictor.h.gcov.overview.html | 159 + .../include/mrs_lib/repredictor.h.gcov.png | Bin 0 -> 2387 bytes .../safety_zone/index-detail-sort-f.html | 112 + .../safety_zone/index-detail-sort-l.html | 112 + .../mrs_lib/safety_zone/index-detail.html | 112 + .../mrs_lib/safety_zone/index-sort-f.html | 112 + .../mrs_lib/safety_zone/index-sort-l.html | 112 + .../include/mrs_lib/safety_zone/index.html | 112 + .../safety_zone/polygon.h.func-sort-c.html | 92 + .../mrs_lib/safety_zone/polygon.h.func.html | 92 + .../safety_zone/polygon.h.gcov.frameset.html | 19 + .../mrs_lib/safety_zone/polygon.h.gcov.html | 136 + .../safety_zone/polygon.h.gcov.overview.html | 33 + .../mrs_lib/safety_zone/polygon.h.gcov.png | Bin 0 -> 337 bytes .../safety_zone.h.func-sort-c.html | 84 + .../safety_zone/safety_zone.h.func.html | 84 + .../safety_zone.h.gcov.frameset.html | 19 + .../safety_zone/safety_zone.h.gcov.html | 121 + .../safety_zone.h.gcov.overview.html | 30 + .../safety_zone/safety_zone.h.gcov.png | Bin 0 -> 280 bytes .../mrs_lib/scope_timer.h.func-sort-c.html | 92 + .../include/mrs_lib/scope_timer.h.func.html | 92 + .../mrs_lib/scope_timer.h.gcov.frameset.html | 19 + .../include/mrs_lib/scope_timer.h.gcov.html | 248 + .../mrs_lib/scope_timer.h.gcov.overview.html | 61 + .../include/mrs_lib/scope_timer.h.gcov.png | Bin 0 -> 777 bytes .../service_client_handler.h.func-sort-c.html | 176 + .../service_client_handler.h.func.html | 176 + ...ervice_client_handler.h.gcov.frameset.html | 19 + .../service_client_handler.h.gcov.html | 327 + ...ervice_client_handler.h.gcov.overview.html | 81 + .../mrs_lib/service_client_handler.h.gcov.png | Bin 0 -> 689 bytes .../subscribe_handler.h.func-sort-c.html | 3056 ++++++ .../mrs_lib/subscribe_handler.h.func.html | 3056 ++++++ .../subscribe_handler.h.gcov.frameset.html | 19 + .../mrs_lib/subscribe_handler.h.gcov.html | 563 ++ .../subscribe_handler.h.gcov.overview.html | 140 + .../mrs_lib/subscribe_handler.h.gcov.png | Bin 0 -> 1968 bytes .../timeout_manager.h.func-sort-c.html | 80 + .../mrs_lib/timeout_manager.h.func.html | 80 + .../timeout_manager.h.gcov.frameset.html | 19 + .../mrs_lib/timeout_manager.h.gcov.html | 169 + .../timeout_manager.h.gcov.overview.html | 42 + .../mrs_lib/timeout_manager.h.gcov.png | Bin 0 -> 435 bytes .../include/mrs_lib/timer.h.func-sort-c.html | 108 + mrs_lib/include/mrs_lib/timer.h.func.html | 108 + .../mrs_lib/timer.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/timer.h.gcov.html | 353 + .../mrs_lib/timer.h.gcov.overview.html | 88 + mrs_lib/include/mrs_lib/timer.h.gcov.png | Bin 0 -> 930 bytes .../mrs_lib/transformer.h.func-sort-c.html | 152 + .../include/mrs_lib/transformer.h.func.html | 152 + .../mrs_lib/transformer.h.gcov.frameset.html | 19 + .../include/mrs_lib/transformer.h.gcov.html | 764 ++ .../mrs_lib/transformer.h.gcov.overview.html | 190 + .../include/mrs_lib/transformer.h.gcov.png | Bin 0 -> 2328 bytes .../include/mrs_lib/ukf.h.func-sort-c.html | 88 + mrs_lib/include/mrs_lib/ukf.h.func.html | 88 + .../include/mrs_lib/ukf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/ukf.h.gcov.html | 285 + .../include/mrs_lib/ukf.h.gcov.overview.html | 71 + mrs_lib/include/mrs_lib/ukf.h.gcov.png | Bin 0 -> 1009 bytes .../include/mrs_lib/utils.h.func-sort-c.html | 100 + mrs_lib/include/mrs_lib/utils.h.func.html | 100 + .../mrs_lib/utils.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/utils.h.gcov.html | 229 + .../mrs_lib/utils.h.gcov.overview.html | 57 + mrs_lib/include/mrs_lib/utils.h.gcov.png | Bin 0 -> 677 bytes .../vector_converter.h.func-sort-c.html | 224 + .../mrs_lib/vector_converter.h.func.html | 224 + .../vector_converter.h.gcov.frameset.html | 19 + .../mrs_lib/vector_converter.h.gcov.html | 128 + .../vector_converter.h.gcov.overview.html | 31 + .../mrs_lib/vector_converter.h.gcov.png | Bin 0 -> 366 bytes .../mrs_lib/visual_object.h.func-sort-c.html | 84 + .../include/mrs_lib/visual_object.h.func.html | 84 + .../visual_object.h.gcov.frameset.html | 19 + .../include/mrs_lib/visual_object.h.gcov.html | 186 + .../visual_object.h.gcov.overview.html | 46 + .../include/mrs_lib/visual_object.h.gcov.png | Bin 0 -> 493 bytes .../attitude_converter.cpp.func-sort-c.html | 244 + .../attitude_converter.cpp.func.html | 244 + .../attitude_converter.cpp.gcov.frameset.html | 19 + .../attitude_converter.cpp.gcov.html | 561 ++ .../attitude_converter.cpp.gcov.overview.html | 140 + .../attitude_converter.cpp.gcov.png | Bin 0 -> 1533 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../src/attitude_converter/index-detail.html | 110 + .../src/attitude_converter/index-sort-f.html | 102 + .../src/attitude_converter/index-sort-l.html | 102 + mrs_lib/src/attitude_converter/index.html | 102 + .../batch_visualizer.cpp.func-sort-c.html | 172 + .../batch_visualizer.cpp.func.html | 172 + .../batch_visualizer.cpp.gcov.frameset.html | 19 + .../batch_visualizer.cpp.gcov.html | 440 + .../batch_visualizer.cpp.gcov.overview.html | 109 + .../batch_visualizer.cpp.gcov.png | Bin 0 -> 1265 bytes .../batch_visualizer/index-detail-sort-f.html | 128 + .../batch_visualizer/index-detail-sort-l.html | 128 + .../src/batch_visualizer/index-detail.html | 128 + .../src/batch_visualizer/index-sort-f.html | 112 + .../src/batch_visualizer/index-sort-l.html | 112 + mrs_lib/src/batch_visualizer/index.html | 112 + .../visual_object.cpp.func-sort-c.html | 168 + .../visual_object.cpp.func.html | 168 + .../visual_object.cpp.gcov.frameset.html | 19 + .../visual_object.cpp.gcov.html | 435 + .../visual_object.cpp.gcov.overview.html | 108 + .../visual_object.cpp.gcov.png | Bin 0 -> 1229 bytes .../geometry/conversions.cpp.func-sort-c.html | 120 + .../src/geometry/conversions.cpp.func.html | 120 + .../conversions.cpp.gcov.frameset.html | 19 + .../src/geometry/conversions.cpp.gcov.html | 178 + .../conversions.cpp.gcov.overview.html | 44 + mrs_lib/src/geometry/conversions.cpp.gcov.png | Bin 0 -> 446 bytes mrs_lib/src/geometry/index-detail-sort-f.html | 138 + mrs_lib/src/geometry/index-detail-sort-l.html | 138 + mrs_lib/src/geometry/index-detail.html | 138 + mrs_lib/src/geometry/index-sort-f.html | 122 + mrs_lib/src/geometry/index-sort-l.html | 122 + mrs_lib/src/geometry/index.html | 122 + .../src/geometry/misc.cpp.func-sort-c.html | 144 + mrs_lib/src/geometry/misc.cpp.func.html | 144 + .../src/geometry/misc.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/misc.cpp.gcov.html | 269 + .../src/geometry/misc.cpp.gcov.overview.html | 67 + mrs_lib/src/geometry/misc.cpp.gcov.png | Bin 0 -> 747 bytes .../src/geometry/shapes.cpp.func-sort-c.html | 352 + mrs_lib/src/geometry/shapes.cpp.func.html | 352 + .../geometry/shapes.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/shapes.cpp.gcov.html | 730 ++ .../geometry/shapes.cpp.gcov.overview.html | 182 + mrs_lib/src/geometry/shapes.cpp.gcov.png | Bin 0 -> 1596 bytes mrs_lib/src/math/index-detail-sort-f.html | 110 + mrs_lib/src/math/index-detail-sort-l.html | 110 + mrs_lib/src/math/index-detail.html | 110 + mrs_lib/src/math/index-sort-f.html | 102 + mrs_lib/src/math/index-sort-l.html | 102 + mrs_lib/src/math/index.html | 102 + mrs_lib/src/math/math.cpp.func-sort-c.html | 84 + mrs_lib/src/math/math.cpp.func.html | 84 + mrs_lib/src/math/math.cpp.gcov.frameset.html | 19 + mrs_lib/src/math/math.cpp.gcov.html | 149 + mrs_lib/src/math/math.cpp.gcov.overview.html | 37 + mrs_lib/src/math/math.cpp.gcov.png | Bin 0 -> 388 bytes .../median_filter/index-detail-sort-f.html | 110 + .../median_filter/index-detail-sort-l.html | 110 + mrs_lib/src/median_filter/index-detail.html | 110 + mrs_lib/src/median_filter/index-sort-f.html | 102 + mrs_lib/src/median_filter/index-sort-l.html | 102 + mrs_lib/src/median_filter/index.html | 102 + .../median_filter.cpp.func-sort-c.html | 148 + .../median_filter/median_filter.cpp.func.html | 148 + .../median_filter.cpp.gcov.frameset.html | 19 + .../median_filter/median_filter.cpp.gcov.html | 286 + .../median_filter.cpp.gcov.overview.html | 71 + .../median_filter/median_filter.cpp.gcov.png | Bin 0 -> 872 bytes .../src/param_loader/index-detail-sort-f.html | 110 + .../src/param_loader/index-detail-sort-l.html | 110 + mrs_lib/src/param_loader/index-detail.html | 110 + mrs_lib/src/param_loader/index-sort-f.html | 102 + mrs_lib/src/param_loader/index-sort-l.html | 102 + mrs_lib/src/param_loader/index.html | 102 + .../param_provider.cpp.func-sort-c.html | 96 + .../param_loader/param_provider.cpp.func.html | 96 + .../param_provider.cpp.gcov.frameset.html | 19 + .../param_loader/param_provider.cpp.gcov.html | 200 + .../param_provider.cpp.gcov.overview.html | 49 + .../param_loader/param_provider.cpp.gcov.png | Bin 0 -> 659 bytes mrs_lib/src/profiler/index-detail-sort-f.html | 110 + mrs_lib/src/profiler/index-detail-sort-l.html | 110 + mrs_lib/src/profiler/index-detail.html | 110 + mrs_lib/src/profiler/index-sort-f.html | 102 + mrs_lib/src/profiler/index-sort-l.html | 102 + mrs_lib/src/profiler/index.html | 102 + .../profiler/profiler.cpp.func-sort-c.html | 120 + mrs_lib/src/profiler/profiler.cpp.func.html | 120 + .../profiler/profiler.cpp.gcov.frameset.html | 19 + mrs_lib/src/profiler/profiler.cpp.gcov.html | 301 + .../profiler/profiler.cpp.gcov.overview.html | 75 + mrs_lib/src/profiler/profiler.cpp.gcov.png | Bin 0 -> 827 bytes .../src/safety_zone/index-detail-sort-f.html | 128 + .../src/safety_zone/index-detail-sort-l.html | 128 + mrs_lib/src/safety_zone/index-detail.html | 128 + mrs_lib/src/safety_zone/index-sort-f.html | 112 + mrs_lib/src/safety_zone/index-sort-l.html | 112 + mrs_lib/src/safety_zone/index.html | 112 + .../line_operations.cpp.func-sort-c.html | 92 + .../safety_zone/line_operations.cpp.func.html | 92 + .../line_operations.cpp.gcov.frameset.html | 19 + .../safety_zone/line_operations.cpp.gcov.html | 143 + .../line_operations.cpp.gcov.overview.html | 35 + .../safety_zone/line_operations.cpp.gcov.png | Bin 0 -> 465 bytes .../polygon/index-detail-sort-f.html | 110 + .../polygon/index-detail-sort-l.html | 110 + .../src/safety_zone/polygon/index-detail.html | 110 + .../src/safety_zone/polygon/index-sort-f.html | 102 + .../src/safety_zone/polygon/index-sort-l.html | 102 + mrs_lib/src/safety_zone/polygon/index.html | 102 + .../polygon/polygon.cpp.func-sort-c.html | 112 + .../safety_zone/polygon/polygon.cpp.func.html | 112 + .../polygon/polygon.cpp.gcov.frameset.html | 19 + .../safety_zone/polygon/polygon.cpp.gcov.html | 290 + .../polygon/polygon.cpp.gcov.overview.html | 72 + .../safety_zone/polygon/polygon.cpp.gcov.png | Bin 0 -> 1057 bytes .../safety_zone.cpp.func-sort-c.html | 104 + .../src/safety_zone/safety_zone.cpp.func.html | 104 + .../safety_zone.cpp.gcov.frameset.html | 19 + .../src/safety_zone/safety_zone.cpp.gcov.html | 160 + .../safety_zone.cpp.gcov.overview.html | 39 + .../src/safety_zone/safety_zone.cpp.gcov.png | Bin 0 -> 380 bytes .../src/scope_timer/index-detail-sort-f.html | 110 + .../src/scope_timer/index-detail-sort-l.html | 110 + mrs_lib/src/scope_timer/index-detail.html | 110 + mrs_lib/src/scope_timer/index-sort-f.html | 102 + mrs_lib/src/scope_timer/index-sort-l.html | 102 + mrs_lib/src/scope_timer/index.html | 102 + .../scope_timer.cpp.func-sort-c.html | 120 + .../src/scope_timer/scope_timer.cpp.func.html | 120 + .../scope_timer.cpp.gcov.frameset.html | 19 + .../src/scope_timer/scope_timer.cpp.gcov.html | 345 + .../scope_timer.cpp.gcov.overview.html | 86 + .../src/scope_timer/scope_timer.cpp.gcov.png | Bin 0 -> 1272 bytes .../timeout_manager/index-detail-sort-f.html | 110 + .../timeout_manager/index-detail-sort-l.html | 110 + mrs_lib/src/timeout_manager/index-detail.html | 110 + mrs_lib/src/timeout_manager/index-sort-f.html | 102 + mrs_lib/src/timeout_manager/index-sort-l.html | 102 + mrs_lib/src/timeout_manager/index.html | 102 + .../timeout_manager.cpp.func-sort-c.html | 124 + .../timeout_manager.cpp.func.html | 124 + .../timeout_manager.cpp.gcov.frameset.html | 19 + .../timeout_manager.cpp.gcov.html | 193 + .../timeout_manager.cpp.gcov.overview.html | 48 + .../timeout_manager.cpp.gcov.png | Bin 0 -> 552 bytes mrs_lib/src/timer/index-detail-sort-f.html | 110 + mrs_lib/src/timer/index-detail-sort-l.html | 110 + mrs_lib/src/timer/index-detail.html | 110 + mrs_lib/src/timer/index-sort-f.html | 102 + mrs_lib/src/timer/index-sort-l.html | 102 + mrs_lib/src/timer/index.html | 102 + mrs_lib/src/timer/timer.cpp.func-sort-c.html | 152 + mrs_lib/src/timer/timer.cpp.func.html | 152 + .../src/timer/timer.cpp.gcov.frameset.html | 19 + mrs_lib/src/timer/timer.cpp.gcov.html | 364 + .../src/timer/timer.cpp.gcov.overview.html | 90 + mrs_lib/src/timer/timer.cpp.gcov.png | Bin 0 -> 1150 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../transform_broadcaster/index-detail.html | 110 + .../transform_broadcaster/index-sort-f.html | 102 + .../transform_broadcaster/index-sort-l.html | 102 + mrs_lib/src/transform_broadcaster/index.html | 102 + ...transform_broadcaster.cpp.func-sort-c.html | 92 + .../transform_broadcaster.cpp.func.html | 92 + ...ansform_broadcaster.cpp.gcov.frameset.html | 19 + .../transform_broadcaster.cpp.gcov.html | 140 + ...ansform_broadcaster.cpp.gcov.overview.html | 34 + .../transform_broadcaster.cpp.gcov.png | Bin 0 -> 425 bytes .../src/transformer/index-detail-sort-f.html | 110 + .../src/transformer/index-detail-sort-l.html | 110 + mrs_lib/src/transformer/index-detail.html | 110 + mrs_lib/src/transformer/index-sort-f.html | 102 + mrs_lib/src/transformer/index-sort-l.html | 102 + mrs_lib/src/transformer/index.html | 102 + .../transformer.cpp.func-sort-c.html | 184 + .../src/transformer/transformer.cpp.func.html | 184 + .../transformer.cpp.gcov.frameset.html | 19 + .../src/transformer/transformer.cpp.gcov.html | 671 ++ .../transformer.cpp.gcov.overview.html | 167 + .../src/transformer/transformer.cpp.gcov.png | Bin 0 -> 1995 bytes mrs_lib/src/utils/index-detail-sort-f.html | 110 + mrs_lib/src/utils/index-detail-sort-l.html | 110 + mrs_lib/src/utils/index-detail.html | 110 + mrs_lib/src/utils/index-sort-f.html | 102 + mrs_lib/src/utils/index-sort-l.html | 102 + mrs_lib/src/utils/index.html | 102 + mrs_lib/src/utils/utils.cpp.func-sort-c.html | 88 + mrs_lib/src/utils/utils.cpp.func.html | 88 + .../src/utils/utils.cpp.gcov.frameset.html | 19 + mrs_lib/src/utils/utils.cpp.gcov.html | 101 + .../src/utils/utils.cpp.gcov.overview.html | 25 + mrs_lib/src/utils/utils.cpp.gcov.png | Bin 0 -> 194 bytes .../src/automatic_start.cpp.func-sort-c.html | 168 + .../src/automatic_start.cpp.func.html | 168 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 1077 ++ .../automatic_start.cpp.gcov.overview.html | 269 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 3175 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + mrs_uav_autostart/src/index-detail.html | 110 + mrs_uav_autostart/src/index-sort-f.html | 102 + mrs_uav_autostart/src/index-sort-l.html | 102 + mrs_uav_autostart/src/index.html | 102 + .../include/common.h.func-sort-c.html | 116 + .../include/common.h.func.html | 116 + .../include/common.h.gcov.frameset.html | 19 + .../include/common.h.gcov.html | 189 + .../include/common.h.gcov.overview.html | 47 + mrs_uav_controllers/include/common.h.gcov.png | Bin 0 -> 436 bytes .../include/index-detail-sort-f.html | 128 + .../include/index-detail-sort-l.html | 128 + mrs_uav_controllers/include/index-detail.html | 128 + mrs_uav_controllers/include/index-sort-f.html | 112 + mrs_uav_controllers/include/index-sort-l.html | 112 + mrs_uav_controllers/include/index.html | 112 + .../include/pid.hpp.func-sort-c.html | 100 + mrs_uav_controllers/include/pid.hpp.func.html | 100 + .../include/pid.hpp.gcov.frameset.html | 19 + mrs_uav_controllers/include/pid.hpp.gcov.html | 184 + .../include/pid.hpp.gcov.overview.html | 45 + mrs_uav_controllers/include/pid.hpp.gcov.png | Bin 0 -> 519 bytes .../src/common.cpp.func-sort-c.html | 116 + mrs_uav_controllers/src/common.cpp.func.html | 116 + .../src/common.cpp.gcov.frameset.html | 19 + mrs_uav_controllers/src/common.cpp.gcov.html | 471 + .../src/common.cpp.gcov.overview.html | 117 + mrs_uav_controllers/src/common.cpp.gcov.png | Bin 0 -> 1368 bytes .../failsafe_controller.cpp.func-sort-c.html | 124 + .../src/failsafe_controller.cpp.func.html | 124 + ...failsafe_controller.cpp.gcov.frameset.html | 19 + .../src/failsafe_controller.cpp.gcov.html | 635 ++ ...failsafe_controller.cpp.gcov.overview.html | 158 + .../src/failsafe_controller.cpp.gcov.png | Bin 0 -> 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 | 2217 ++++ .../src/mpc_controller.cpp.gcov.overview.html | 554 + .../src/mpc_controller.cpp.gcov.png | Bin 0 -> 6670 bytes .../src/se3_controller.cpp.func-sort-c.html | 148 + .../src/se3_controller.cpp.func.html | 148 + .../src/se3_controller.cpp.gcov.frameset.html | 19 + .../src/se3_controller.cpp.gcov.html | 1930 ++++ .../src/se3_controller.cpp.gcov.overview.html | 482 + .../src/se3_controller.cpp.gcov.png | Bin 0 -> 5854 bytes .../control_manager/index-detail-sort-f.html | 110 + .../control_manager/index-detail-sort-l.html | 110 + .../include/control_manager/index-detail.html | 110 + .../include/control_manager/index-sort-f.html | 102 + .../include/control_manager/index-sort-l.html | 102 + .../include/control_manager/index.html | 102 + .../output_publisher.h.func-sort-c.html | 120 + .../output_publisher.h.func.html | 120 + .../output_publisher.h.gcov.frameset.html | 19 + .../output_publisher.h.gcov.html | 146 + .../output_publisher.h.gcov.overview.html | 36 + .../output_publisher.h.gcov.png | Bin 0 -> 332 bytes .../agl_estimator.h.func-sort-c.html | 92 + .../agl_estimator.h.func.html | 92 + .../agl_estimator.h.gcov.frameset.html | 19 + .../agl_estimator.h.gcov.html | 157 + .../agl_estimator.h.gcov.overview.html | 39 + .../mrs_uav_managers/agl_estimator.h.gcov.png | Bin 0 -> 397 bytes .../control_manager/common.h.func-sort-c.html | 188 + .../control_manager/common.h.func.html | 188 + .../common.h.gcov.frameset.html | 19 + .../control_manager/common.h.gcov.html | 380 + .../common.h.gcov.overview.html | 94 + .../control_manager/common.h.gcov.png | Bin 0 -> 936 bytes .../control_manager/index-detail-sort-f.html | 110 + .../control_manager/index-detail-sort-l.html | 110 + .../control_manager/index-detail.html | 110 + .../control_manager/index-sort-f.html | 102 + .../control_manager/index-sort-l.html | 102 + .../control_manager/index.html | 102 + .../controller.h.func-sort-c.html | 88 + .../mrs_uav_managers/controller.h.func.html | 88 + .../controller.h.gcov.frameset.html | 19 + .../mrs_uav_managers/controller.h.gcov.html | 232 + .../controller.h.gcov.overview.html | 57 + .../mrs_uav_managers/controller.h.gcov.png | Bin 0 -> 681 bytes .../estimator.h.func-sort-c.html | 92 + .../estimation_manager/estimator.h.func.html | 92 + .../estimator.h.gcov.frameset.html | 19 + .../estimation_manager/estimator.h.gcov.html | 195 + .../estimator.h.gcov.overview.html | 48 + .../estimation_manager/estimator.h.gcov.png | Bin 0 -> 506 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../estimation_manager/index-detail.html | 128 + .../estimation_manager/index-sort-f.html | 112 + .../estimation_manager/index-sort-l.html | 112 + .../estimation_manager/index.html | 112 + .../support.h.func-sort-c.html | 140 + .../estimation_manager/support.h.func.html | 140 + .../support.h.gcov.frameset.html | 19 + .../estimation_manager/support.h.gcov.html | 405 + .../support.h.gcov.overview.html | 101 + .../estimation_manager/support.h.gcov.png | Bin 0 -> 1232 bytes .../mrs_uav_managers/index-detail-sort-f.html | 164 + .../mrs_uav_managers/index-detail-sort-l.html | 164 + .../mrs_uav_managers/index-detail.html | 164 + .../mrs_uav_managers/index-sort-f.html | 132 + .../mrs_uav_managers/index-sort-l.html | 132 + .../include/mrs_uav_managers/index.html | 132 + .../state_estimator.h.func-sort-c.html | 92 + .../state_estimator.h.func.html | 92 + .../state_estimator.h.gcov.frameset.html | 19 + .../state_estimator.h.gcov.html | 169 + .../state_estimator.h.gcov.overview.html | 42 + .../state_estimator.h.gcov.png | Bin 0 -> 435 bytes .../tracker.h.func-sort-c.html | 88 + .../mrs_uav_managers/tracker.h.func.html | 88 + .../tracker.h.gcov.frameset.html | 19 + .../mrs_uav_managers/tracker.h.gcov.html | 296 + .../tracker.h.gcov.overview.html | 73 + .../mrs_uav_managers/tracker.h.gcov.png | Bin 0 -> 751 bytes .../index-detail-sort-f.html | 120 + .../index-detail-sort-l.html | 120 + .../transform_manager/index-detail.html | 120 + .../transform_manager/index-sort-f.html | 112 + .../transform_manager/index-sort-l.html | 112 + .../include/transform_manager/index.html | 112 + .../tf_mapping_origin.h.func-sort-c.html | 100 + .../tf_mapping_origin.h.func.html | 100 + .../tf_mapping_origin.h.gcov.frameset.html | 19 + .../tf_mapping_origin.h.gcov.html | 474 + .../tf_mapping_origin.h.gcov.overview.html | 118 + .../tf_mapping_origin.h.gcov.png | Bin 0 -> 1536 bytes .../tf_source.h.func-sort-c.html | 136 + .../transform_manager/tf_source.h.func.html | 136 + .../tf_source.h.gcov.frameset.html | 19 + .../transform_manager/tf_source.h.gcov.html | 704 ++ .../tf_source.h.gcov.overview.html | 175 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2213 bytes .../constraint_manager.cpp.func-sort-c.html | 112 + .../src/constraint_manager.cpp.func.html | 112 + .../constraint_manager.cpp.gcov.frameset.html | 19 + .../src/constraint_manager.cpp.gcov.html | 716 ++ .../constraint_manager.cpp.gcov.overview.html | 178 + .../src/constraint_manager.cpp.gcov.png | Bin 0 -> 2190 bytes .../common/common.cpp.func-sort-c.html | 204 + .../common/common.cpp.func.html | 204 + .../common/common.cpp.gcov.frameset.html | 19 + .../common/common.cpp.gcov.html | 1312 +++ .../common/common.cpp.gcov.overview.html | 327 + .../common/common.cpp.gcov.png | Bin 0 -> 2428 bytes .../common/index-detail-sort-f.html | 110 + .../common/index-detail-sort-l.html | 110 + .../control_manager/common/index-detail.html | 110 + .../control_manager/common/index-sort-f.html | 102 + .../control_manager/common/index-sort-l.html | 102 + .../src/control_manager/common/index.html | 102 + .../control_manager.cpp.func-sort-c.html | 524 + .../control_manager.cpp.func.html | 524 + .../control_manager.cpp.gcov.frameset.html | 19 + .../control_manager.cpp.gcov.html | 8941 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2235 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 25927 bytes .../control_manager/index-detail-sort-f.html | 128 + .../control_manager/index-detail-sort-l.html | 128 + .../src/control_manager/index-detail.html | 128 + .../src/control_manager/index-sort-f.html | 112 + .../src/control_manager/index-sort-l.html | 112 + .../src/control_manager/index.html | 112 + .../output_publisher.cpp.func-sort-c.html | 128 + .../output_publisher.cpp.func.html | 128 + .../output_publisher.cpp.gcov.frameset.html | 19 + .../output_publisher.cpp.gcov.html | 154 + .../output_publisher.cpp.gcov.overview.html | 38 + .../output_publisher.cpp.gcov.png | Bin 0 -> 307 bytes .../estimation_manager.cpp.func-sort-c.html | 176 + .../estimation_manager.cpp.func.html | 176 + .../estimation_manager.cpp.gcov.frameset.html | 19 + .../estimation_manager.cpp.gcov.html | 1338 +++ .../estimation_manager.cpp.gcov.overview.html | 334 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4355 bytes .../estimator.cpp.func-sort-c.html | 172 + .../estimator.cpp.func.html | 172 + .../estimator.cpp.gcov.frameset.html | 19 + .../estimator.cpp.gcov.html | 295 + .../estimator.cpp.gcov.overview.html | 73 + .../estimation_manager/estimator.cpp.gcov.png | Bin 0 -> 824 bytes .../agl_estimator.cpp.func-sort-c.html | 92 + .../estimators/agl_estimator.cpp.func.html | 92 + .../agl_estimator.cpp.gcov.frameset.html | 19 + .../estimators/agl_estimator.cpp.gcov.html | 182 + .../agl_estimator.cpp.gcov.overview.html | 45 + .../estimators/agl_estimator.cpp.gcov.png | Bin 0 -> 454 bytes .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimation_manager/estimators/index.html | 112 + .../state_estimator.cpp.func-sort-c.html | 120 + .../estimators/state_estimator.cpp.func.html | 120 + .../state_estimator.cpp.gcov.frameset.html | 19 + .../estimators/state_estimator.cpp.gcov.html | 263 + .../state_estimator.cpp.gcov.overview.html | 65 + .../estimators/state_estimator.cpp.gcov.png | Bin 0 -> 753 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../src/estimation_manager/index-detail.html | 128 + .../src/estimation_manager/index-sort-f.html | 112 + .../src/estimation_manager/index-sort-l.html | 112 + .../src/estimation_manager/index.html | 112 + .../src/gain_manager.cpp.func-sort-c.html | 108 + .../src/gain_manager.cpp.func.html | 108 + .../src/gain_manager.cpp.gcov.frameset.html | 19 + .../src/gain_manager.cpp.gcov.html | 730 ++ .../src/gain_manager.cpp.gcov.overview.html | 182 + .../src/gain_manager.cpp.gcov.png | Bin 0 -> 2153 bytes mrs_uav_managers/src/index-detail-sort-f.html | 164 + mrs_uav_managers/src/index-detail-sort-l.html | 164 + mrs_uav_managers/src/index-detail.html | 164 + mrs_uav_managers/src/index-sort-f.html | 132 + mrs_uav_managers/src/index-sort-l.html | 132 + mrs_uav_managers/src/index.html | 132 + .../src/null_tracker.cpp.func-sort-c.html | 160 + .../src/null_tracker.cpp.func.html | 160 + .../src/null_tracker.cpp.gcov.frameset.html | 19 + .../src/null_tracker.cpp.gcov.html | 332 + .../src/null_tracker.cpp.gcov.overview.html | 82 + .../src/null_tracker.cpp.gcov.png | Bin 0 -> 809 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../src/transform_manager/index-detail.html | 110 + .../src/transform_manager/index-sort-f.html | 102 + .../src/transform_manager/index-sort-l.html | 102 + .../src/transform_manager/index.html | 102 + .../transform_manager.cpp.func-sort-c.html | 136 + .../transform_manager.cpp.func.html | 136 + .../transform_manager.cpp.gcov.frameset.html | 19 + .../transform_manager.cpp.gcov.html | 1038 ++ .../transform_manager.cpp.gcov.overview.html | 259 + .../transform_manager.cpp.gcov.png | Bin 0 -> 3381 bytes .../src/uav_manager.cpp.func-sort-c.html | 232 + .../src/uav_manager.cpp.func.html | 232 + .../src/uav_manager.cpp.gcov.frameset.html | 19 + .../src/uav_manager.cpp.gcov.html | 2765 +++++ .../src/uav_manager.cpp.gcov.overview.html | 691 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7293 bytes .../agl/garmin_agl.h.func-sort-c.html | 92 + .../estimators/agl/garmin_agl.h.func.html | 92 + .../agl/garmin_agl.h.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.h.gcov.html | 159 + .../agl/garmin_agl.h.gcov.overview.html | 39 + .../estimators/agl/garmin_agl.h.gcov.png | Bin 0 -> 380 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../estimators/agl/index-detail.html | 110 + .../estimators/agl/index-sort-f.html | 102 + .../estimators/agl/index-sort-l.html | 102 + .../estimators/agl/index.html | 102 + .../altitude/alt_generic.h.func-sort-c.html | 92 + .../altitude/alt_generic.h.func.html | 92 + .../altitude/alt_generic.h.gcov.frameset.html | 19 + .../altitude/alt_generic.h.gcov.html | 245 + .../altitude/alt_generic.h.gcov.overview.html | 61 + .../altitude/alt_generic.h.gcov.png | Bin 0 -> 688 bytes .../altitude_estimator.h.func-sort-c.html | 92 + .../altitude/altitude_estimator.h.func.html | 92 + .../altitude_estimator.h.gcov.frameset.html | 19 + .../altitude/altitude_estimator.h.gcov.html | 130 + .../altitude_estimator.h.gcov.overview.html | 32 + .../altitude/altitude_estimator.h.gcov.png | Bin 0 -> 295 bytes .../altitude/index-detail-sort-f.html | 128 + .../altitude/index-detail-sort-l.html | 128 + .../estimators/altitude/index-detail.html | 128 + .../estimators/altitude/index-sort-f.html | 112 + .../estimators/altitude/index-sort-l.html | 112 + .../estimators/altitude/index.html | 112 + .../estimators/correction.h.func-sort-c.html | 364 + .../estimators/correction.h.func.html | 364 + .../correction.h.gcov.frameset.html | 19 + .../estimators/correction.h.gcov.html | 1948 ++++ .../correction.h.gcov.overview.html | 486 + .../estimators/correction.h.gcov.png | Bin 0 -> 5385 bytes .../heading/hdg_generic.h.func-sort-c.html | 92 + .../heading/hdg_generic.h.func.html | 92 + .../heading/hdg_generic.h.gcov.frameset.html | 19 + .../heading/hdg_generic.h.gcov.html | 243 + .../heading/hdg_generic.h.gcov.overview.html | 60 + .../estimators/heading/hdg_generic.h.gcov.png | Bin 0 -> 663 bytes .../hdg_passthrough.h.func-sort-c.html | 92 + .../heading/hdg_passthrough.h.func.html | 92 + .../hdg_passthrough.h.gcov.frameset.html | 19 + .../heading/hdg_passthrough.h.gcov.html | 191 + .../hdg_passthrough.h.gcov.overview.html | 47 + .../heading/hdg_passthrough.h.gcov.png | Bin 0 -> 506 bytes .../heading_estimator.h.func-sort-c.html | 84 + .../heading/heading_estimator.h.func.html | 84 + .../heading_estimator.h.gcov.frameset.html | 19 + .../heading/heading_estimator.h.gcov.html | 124 + .../heading_estimator.h.gcov.overview.html | 30 + .../heading/heading_estimator.h.gcov.png | Bin 0 -> 293 bytes .../heading/index-detail-sort-f.html | 138 + .../heading/index-detail-sort-l.html | 138 + .../estimators/heading/index-detail.html | 138 + .../estimators/heading/index-sort-f.html | 122 + .../estimators/heading/index-sort-l.html | 122 + .../estimators/heading/index.html | 122 + .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimators/index.html | 112 + .../lateral/index-detail-sort-f.html | 128 + .../lateral/index-detail-sort-l.html | 128 + .../estimators/lateral/index-detail.html | 128 + .../estimators/lateral/index-sort-f.html | 112 + .../estimators/lateral/index-sort-l.html | 112 + .../estimators/lateral/index.html | 112 + .../lateral/lat_generic.h.func-sort-c.html | 92 + .../lateral/lat_generic.h.func.html | 92 + .../lateral/lat_generic.h.gcov.frameset.html | 19 + .../lateral/lat_generic.h.gcov.html | 250 + .../lateral/lat_generic.h.gcov.overview.html | 62 + .../estimators/lateral/lat_generic.h.gcov.png | Bin 0 -> 708 bytes .../lateral_estimator.h.func-sort-c.html | 92 + .../lateral/lateral_estimator.h.func.html | 92 + .../lateral_estimator.h.gcov.frameset.html | 19 + .../lateral/lateral_estimator.h.gcov.html | 122 + .../lateral_estimator.h.gcov.overview.html | 30 + .../lateral/lateral_estimator.h.gcov.png | Bin 0 -> 284 bytes .../partial_estimator.h.func-sort-c.html | 176 + .../estimators/partial_estimator.h.func.html | 176 + .../partial_estimator.h.gcov.frameset.html | 19 + .../estimators/partial_estimator.h.gcov.html | 264 + .../partial_estimator.h.gcov.overview.html | 65 + .../estimators/partial_estimator.h.gcov.png | Bin 0 -> 820 bytes .../estimators/state/index-detail-sort-f.html | 128 + .../estimators/state/index-detail-sort-l.html | 128 + .../estimators/state/index-detail.html | 128 + .../estimators/state/index-sort-f.html | 112 + .../estimators/state/index-sort-l.html | 112 + .../estimators/state/index.html | 112 + .../state/passthrough.h.func-sort-c.html | 92 + .../estimators/state/passthrough.h.func.html | 92 + .../state/passthrough.h.gcov.frameset.html | 19 + .../estimators/state/passthrough.h.gcov.html | 173 + .../state/passthrough.h.gcov.overview.html | 43 + .../estimators/state/passthrough.h.gcov.png | Bin 0 -> 431 bytes .../state/state_generic.h.func-sort-c.html | 92 + .../state/state_generic.h.func.html | 92 + .../state/state_generic.h.gcov.frameset.html | 19 + .../state/state_generic.h.gcov.html | 183 + .../state/state_generic.h.gcov.overview.html | 45 + .../estimators/state/state_generic.h.gcov.png | Bin 0 -> 438 bytes .../processors/index-detail-sort-f.html | 182 + .../processors/index-detail-sort-l.html | 182 + .../processors/index-detail.html | 182 + .../processors/index-sort-f.html | 142 + .../processors/index-sort-l.html | 142 + .../processors/index.html | 142 + .../proc_excessive_tilt.h.func-sort-c.html | 104 + .../proc_excessive_tilt.h.func.html | 104 + .../proc_excessive_tilt.h.gcov.frameset.html | 19 + .../proc_excessive_tilt.h.gcov.html | 206 + .../proc_excessive_tilt.h.gcov.overview.html | 51 + .../processors/proc_excessive_tilt.h.gcov.png | Bin 0 -> 656 bytes .../proc_median_filter.h.func-sort-c.html | 104 + .../processors/proc_median_filter.h.func.html | 104 + .../proc_median_filter.h.gcov.frameset.html | 19 + .../processors/proc_median_filter.h.gcov.html | 192 + .../proc_median_filter.h.gcov.overview.html | 47 + .../processors/proc_median_filter.h.gcov.png | Bin 0 -> 635 bytes .../proc_saturate.h.func-sort-c.html | 104 + .../processors/proc_saturate.h.func.html | 104 + .../proc_saturate.h.gcov.frameset.html | 19 + .../processors/proc_saturate.h.gcov.html | 202 + .../proc_saturate.h.gcov.overview.html | 50 + .../processors/proc_saturate.h.gcov.png | Bin 0 -> 711 bytes .../proc_tf_to_world.h.func-sort-c.html | 112 + .../processors/proc_tf_to_world.h.func.html | 112 + .../proc_tf_to_world.h.gcov.frameset.html | 19 + .../processors/proc_tf_to_world.h.gcov.html | 241 + .../proc_tf_to_world.h.gcov.overview.html | 60 + .../processors/proc_tf_to_world.h.gcov.png | Bin 0 -> 777 bytes .../processors/processor.h.func-sort-c.html | 112 + .../processors/processor.h.func.html | 112 + .../processors/processor.h.gcov.frameset.html | 19 + .../processors/processor.h.gcov.html | 156 + .../processors/processor.h.gcov.overview.html | 38 + .../processors/processor.h.gcov.png | Bin 0 -> 436 bytes .../agl/garmin_agl.cpp.func-sort-c.html | 120 + .../estimators/agl/garmin_agl.cpp.func.html | 120 + .../agl/garmin_agl.cpp.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.cpp.gcov.html | 318 + .../agl/garmin_agl.cpp.gcov.overview.html | 79 + .../estimators/agl/garmin_agl.cpp.gcov.png | Bin 0 -> 1043 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../src/estimators/agl/index-detail.html | 110 + .../src/estimators/agl/index-sort-f.html | 102 + .../src/estimators/agl/index-sort-l.html | 102 + .../src/estimators/agl/index.html | 102 + .../altitude/alt_generic.cpp.func-sort-c.html | 212 + .../altitude/alt_generic.cpp.func.html | 212 + .../alt_generic.cpp.gcov.frameset.html | 19 + .../altitude/alt_generic.cpp.gcov.html | 839 ++ .../alt_generic.cpp.gcov.overview.html | 209 + .../altitude/alt_generic.cpp.gcov.png | Bin 0 -> 2919 bytes .../altitude/index-detail-sort-f.html | 110 + .../altitude/index-detail-sort-l.html | 110 + .../src/estimators/altitude/index-detail.html | 110 + .../src/estimators/altitude/index-sort-f.html | 102 + .../src/estimators/altitude/index-sort-l.html | 102 + .../src/estimators/altitude/index.html | 102 + .../heading/hdg_generic.cpp.func-sort-c.html | 216 + .../heading/hdg_generic.cpp.func.html | 216 + .../hdg_generic.cpp.gcov.frameset.html | 19 + .../heading/hdg_generic.cpp.gcov.html | 795 ++ .../hdg_generic.cpp.gcov.overview.html | 198 + .../heading/hdg_generic.cpp.gcov.png | Bin 0 -> 2310 bytes .../hdg_passthrough.cpp.func-sort-c.html | 172 + .../heading/hdg_passthrough.cpp.func.html | 172 + .../hdg_passthrough.cpp.gcov.frameset.html | 19 + .../heading/hdg_passthrough.cpp.gcov.html | 391 + .../hdg_passthrough.cpp.gcov.overview.html | 97 + .../heading/hdg_passthrough.cpp.gcov.png | Bin 0 -> 1198 bytes .../heading/index-detail-sort-f.html | 120 + .../heading/index-detail-sort-l.html | 120 + .../src/estimators/heading/index-detail.html | 120 + .../src/estimators/heading/index-sort-f.html | 112 + .../src/estimators/heading/index-sort-l.html | 112 + .../src/estimators/heading/index.html | 112 + .../lateral/index-detail-sort-f.html | 110 + .../lateral/index-detail-sort-l.html | 110 + .../src/estimators/lateral/index-detail.html | 110 + .../src/estimators/lateral/index-sort-f.html | 102 + .../src/estimators/lateral/index-sort-l.html | 102 + .../src/estimators/lateral/index.html | 102 + .../lateral/lat_generic.cpp.func-sort-c.html | 212 + .../lateral/lat_generic.cpp.func.html | 212 + .../lat_generic.cpp.gcov.frameset.html | 19 + .../lateral/lat_generic.cpp.gcov.html | 887 ++ .../lat_generic.cpp.gcov.overview.html | 221 + .../lateral/lat_generic.cpp.gcov.png | Bin 0 -> 3149 bytes .../state/gps_baro.cpp.func-sort-c.html | 96 + .../estimators/state/gps_baro.cpp.func.html | 96 + .../state/gps_baro.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_baro.cpp.gcov.html | 109 + .../state/gps_baro.cpp.gcov.overview.html | 27 + .../estimators/state/gps_baro.cpp.gcov.png | Bin 0 -> 231 bytes .../state/gps_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/gps_garmin.cpp.func.html | 96 + .../state/gps_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_garmin.cpp.gcov.html | 109 + .../state/gps_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/gps_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../estimators/state/index-detail-sort-f.html | 200 + .../estimators/state/index-detail-sort-l.html | 200 + .../src/estimators/state/index-detail.html | 200 + .../src/estimators/state/index-sort-f.html | 152 + .../src/estimators/state/index-sort-l.html | 152 + .../src/estimators/state/index.html | 152 + .../state/passthrough.cpp.func-sort-c.html | 116 + .../state/passthrough.cpp.func.html | 116 + .../state/passthrough.cpp.gcov.frameset.html | 19 + .../state/passthrough.cpp.gcov.html | 343 + .../state/passthrough.cpp.gcov.overview.html | 85 + .../estimators/state/passthrough.cpp.gcov.png | Bin 0 -> 1099 bytes .../estimators/state/rtk.cpp.func-sort-c.html | 96 + .../src/estimators/state/rtk.cpp.func.html | 96 + .../state/rtk.cpp.gcov.frameset.html | 19 + .../src/estimators/state/rtk.cpp.gcov.html | 109 + .../state/rtk.cpp.gcov.overview.html | 27 + .../src/estimators/state/rtk.cpp.gcov.png | Bin 0 -> 229 bytes .../state/rtk_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/rtk_garmin.cpp.func.html | 96 + .../state/rtk_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/rtk_garmin.cpp.gcov.html | 109 + .../state/rtk_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/rtk_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../state/state_generic.cpp.func-sort-c.html | 128 + .../state/state_generic.cpp.func.html | 128 + .../state_generic.cpp.gcov.frameset.html | 19 + .../state/state_generic.cpp.gcov.html | 633 ++ .../state_generic.cpp.gcov.overview.html | 158 + .../state/state_generic.cpp.gcov.png | Bin 0 -> 1968 bytes .../src/joy_tracker/index-detail-sort-f.html | 110 + .../src/joy_tracker/index-detail-sort-l.html | 110 + .../src/joy_tracker/index-detail.html | 110 + .../src/joy_tracker/index-sort-f.html | 102 + .../src/joy_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/joy_tracker/index.html | 102 + .../joy_tracker.cpp.func-sort-c.html | 152 + .../src/joy_tracker/joy_tracker.cpp.func.html | 152 + .../joy_tracker.cpp.gcov.frameset.html | 19 + .../src/joy_tracker/joy_tracker.cpp.gcov.html | 588 ++ .../joy_tracker.cpp.gcov.overview.html | 146 + .../src/joy_tracker/joy_tracker.cpp.gcov.png | Bin 0 -> 1740 bytes .../landoff_tracker/index-detail-sort-f.html | 110 + .../landoff_tracker/index-detail-sort-l.html | 110 + .../src/landoff_tracker/index-detail.html | 110 + .../src/landoff_tracker/index-sort-f.html | 102 + .../src/landoff_tracker/index-sort-l.html | 102 + .../src/landoff_tracker/index.html | 102 + .../landoff_tracker.cpp.func-sort-c.html | 204 + .../landoff_tracker.cpp.func.html | 204 + .../landoff_tracker.cpp.gcov.frameset.html | 19 + .../landoff_tracker.cpp.gcov.html | 1645 +++ .../landoff_tracker.cpp.gcov.overview.html | 411 + .../landoff_tracker.cpp.gcov.png | Bin 0 -> 5276 bytes .../src/line_tracker/index-detail-sort-f.html | 110 + .../src/line_tracker/index-detail-sort-l.html | 110 + .../src/line_tracker/index-detail.html | 110 + .../src/line_tracker/index-sort-f.html | 102 + .../src/line_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/line_tracker/index.html | 102 + .../line_tracker.cpp.func-sort-c.html | 200 + .../line_tracker/line_tracker.cpp.func.html | 200 + .../line_tracker.cpp.gcov.frameset.html | 19 + .../line_tracker/line_tracker.cpp.gcov.html | 1364 +++ .../line_tracker.cpp.gcov.overview.html | 340 + .../line_tracker/line_tracker.cpp.gcov.png | Bin 0 -> 4090 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../index-detail.html | 110 + .../index-sort-f.html | 102 + .../index-sort-l.html | 102 + .../src/midair_activation_tracker/index.html | 102 + ...ir_activation_tracker.cpp.func-sort-c.html | 152 + .../midair_activation_tracker.cpp.func.html | 152 + ..._activation_tracker.cpp.gcov.frameset.html | 19 + .../midair_activation_tracker.cpp.gcov.html | 426 + ..._activation_tracker.cpp.gcov.overview.html | 106 + .../midair_activation_tracker.cpp.gcov.png | Bin 0 -> 1153 bytes .../src/mpc_tracker/index-detail-sort-f.html | 110 + .../src/mpc_tracker/index-detail-sort-l.html | 110 + .../src/mpc_tracker/index-detail.html | 110 + .../src/mpc_tracker/index-sort-f.html | 102 + .../src/mpc_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/mpc_tracker/index.html | 102 + .../mpc_tracker.cpp.func-sort-c.html | 280 + .../src/mpc_tracker/mpc_tracker.cpp.func.html | 280 + .../mpc_tracker.cpp.gcov.frameset.html | 19 + .../src/mpc_tracker/mpc_tracker.cpp.gcov.html | 3955 ++++++++ .../mpc_tracker.cpp.gcov.overview.html | 988 ++ .../src/mpc_tracker/mpc_tracker.cpp.gcov.png | Bin 0 -> 13661 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 | 1183 +++ .../speed_tracker.cpp.gcov.overview.html | 295 + .../speed_tracker/speed_tracker.cpp.gcov.png | Bin 0 -> 3298 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + .../src/index-detail.html | 110 + .../src/index-sort-f.html | 102 + .../src/index-sort-l.html | 102 + mrs_uav_trajectory_generation/src/index.html | 102 + ...trajectory_generation.cpp.func-sort-c.html | 168 + .../mrs_trajectory_generation.cpp.func.html | 168 + ...ajectory_generation.cpp.gcov.frameset.html | 19 + .../mrs_trajectory_generation.cpp.gcov.html | 2458 +++++ ...ajectory_generation.cpp.gcov.overview.html | 614 ++ .../mrs_trajectory_generation.cpp.gcov.png | Bin 0 -> 7279 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 1020 files changed, 179503 insertions(+) create mode 100644 .nojekyll create mode 100644 amber.png create mode 100644 badge.svg create mode 100644 emerald.png create mode 100644 gcov.css create mode 100644 glass.png create mode 100644 index-sort-f.html create mode 100644 index-sort-l.html create mode 100644 index.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/utils.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.func.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.png create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.png create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index-detail.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/conversions.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.func.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/src/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/src/geometry/index-detail.html create mode 100644 mrs_lib/src/geometry/index-sort-f.html create mode 100644 mrs_lib/src/geometry/index-sort-l.html create mode 100644 mrs_lib/src/geometry/index.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/shapes.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.func.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.png create mode 100644 mrs_lib/src/math/index-detail-sort-f.html create mode 100644 mrs_lib/src/math/index-detail-sort-l.html create mode 100644 mrs_lib/src/math/index-detail.html create mode 100644 mrs_lib/src/math/index-sort-f.html create mode 100644 mrs_lib/src/math/index-sort-l.html create mode 100644 mrs_lib/src/math/index.html create mode 100644 mrs_lib/src/math/math.cpp.func-sort-c.html create mode 100644 mrs_lib/src/math/math.cpp.func.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.overview.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.png create mode 100644 mrs_lib/src/median_filter/index-detail-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-detail-sort-l.html create mode 100644 mrs_lib/src/median_filter/index-detail.html create mode 100644 mrs_lib/src/median_filter/index-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-sort-l.html create mode 100644 mrs_lib/src/median_filter/index.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.png create mode 100644 mrs_lib/src/param_loader/index-detail-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-detail-sort-l.html create mode 100644 mrs_lib/src/param_loader/index-detail.html create mode 100644 mrs_lib/src/param_loader/index-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-sort-l.html create mode 100644 mrs_lib/src/param_loader/index.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.png create mode 100644 mrs_lib/src/profiler/index-detail-sort-f.html create mode 100644 mrs_lib/src/profiler/index-detail-sort-l.html create mode 100644 mrs_lib/src/profiler/index-detail.html create mode 100644 mrs_lib/src/profiler/index-sort-f.html create mode 100644 mrs_lib/src/profiler/index-sort-l.html create mode 100644 mrs_lib/src/profiler/index.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func-sort-c.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.overview.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index-detail.html create mode 100644 mrs_lib/src/safety_zone/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index-detail.html create mode 100644 mrs_lib/src/scope_timer/index-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index-detail.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png create mode 100644 mrs_lib/src/timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/timer/index-detail.html create mode 100644 mrs_lib/src/timer/index-sort-f.html create mode 100644 mrs_lib/src/timer/index-sort-l.html create mode 100644 mrs_lib/src/timer/index.html create mode 100644 mrs_lib/src/timer/timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timer/timer.cpp.func.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.png create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png create mode 100644 mrs_lib/src/transformer/index-detail-sort-f.html create mode 100644 mrs_lib/src/transformer/index-detail-sort-l.html create mode 100644 mrs_lib/src/transformer/index-detail.html create mode 100644 mrs_lib/src/transformer/index-sort-f.html create mode 100644 mrs_lib/src/transformer/index-sort-l.html create mode 100644 mrs_lib/src/transformer/index.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.png create mode 100644 mrs_lib/src/utils/index-detail-sort-f.html create mode 100644 mrs_lib/src/utils/index-detail-sort-l.html create mode 100644 mrs_lib/src/utils/index-detail.html create mode 100644 mrs_lib/src/utils/index-sort-f.html create mode 100644 mrs_lib/src/utils/index-sort-l.html create mode 100644 mrs_lib/src/utils/index.html create mode 100644 mrs_lib/src/utils/utils.cpp.func-sort-c.html create mode 100644 mrs_lib/src/utils/utils.cpp.func.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.overview.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func-sort-c.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/index-detail-sort-f.html create mode 100644 mrs_uav_autostart/src/index-detail-sort-l.html create mode 100644 mrs_uav_autostart/src/index-detail.html create mode 100644 mrs_uav_autostart/src/index-sort-f.html create mode 100644 mrs_uav_autostart/src/index-sort-l.html create mode 100644 mrs_uav_autostart/src/index.html create mode 100644 mrs_uav_controllers/include/common.h.func-sort-c.html create mode 100644 mrs_uav_controllers/include/common.h.func.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.overview.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.png create mode 100644 mrs_uav_controllers/include/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/include/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/include/index-detail.html create mode 100644 mrs_uav_controllers/include/index-sort-f.html create mode 100644 mrs_uav_controllers/include/index-sort-l.html create mode 100644 mrs_uav_controllers/include/index.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func-sort-c.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.overview.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.png create mode 100644 mrs_uav_controllers/src/common.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/common.cpp.func.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/src/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/src/index-detail.html create mode 100644 mrs_uav_controllers/src/index-sort-f.html create mode 100644 mrs_uav_controllers/src/index-sort-l.html create mode 100644 mrs_uav_controllers/src/index.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.png create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/index-detail.html create mode 100644 mrs_uav_managers/src/index-sort-f.html create mode 100644 mrs_uav_managers/src/index-sort-l.html create mode 100644 mrs_uav_managers/src/index.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.png create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/index.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png create mode 100644 ruby.png create mode 100644 snow.png create mode 100644 updown.png diff --git a/.nojekyll b/.nojekyll new file mode 100644 index 0000000000..e69de29bb2 diff --git a/amber.png b/amber.png new file mode 100644 index 0000000000000000000000000000000000000000..2cab170d8359081983a4e343848dfe06bc490f12 GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^G2tW}LqE04T&+ z;1OBOz`!j8!i<;h*8KqrvZOouIx;Y9?C1WI$O`1M1^9%x{(levWGtest coveragetest coverage63.3%63.3% \ 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..9291eca908 --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:143212139466.9 %
Date:2024-06-08 22:13:12Functions:2641417363.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
13.4%13.4%
+ 		13.4 %19 / 14250.0 %5 / 10
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.7%71.7%
+ 		71.7 %132 / 18450.0 %17 / 34
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44659.5 %830 / 1394
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98562.5 %846 / 1354
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_managers/include/transform_manager +
48.2%48.2%
+ 		48.2 %192 / 39868.4 %13 / 19
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.1%53.1%
+ 		53.1 %405 / 76369.5 %66 / 95
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_uav_managers/src/estimation_manager +
69.9%69.9%
+ 		69.9 %437 / 62585.1 %40 / 47
mrs_uav_managers/src/control_manager +
69.8%69.8%
+ 		69.8 %2594 / 371487.0 %107 / 123
mrs_uav_managers/src +
72.6%72.6%
+ 		72.6 %1247 / 171887.7 %64 / 73
mrs_uav_controllers/src +
80.9%80.9%
+ 		80.9 %1759 / 217487.9 %58 / 66
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/profiler +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
mrs_uav_trackers/src/mpc_tracker +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
mrs_uav_managers/src/transform_manager +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_autostart/src +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index-sort-l.html b/index-sort-l.html new file mode 100644 index 0000000000..a9cc42374d --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:143212139466.9 %
Date:2024-06-08 22:13:12Functions:2641417363.3 %
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 +
13.4%13.4%
+ 		13.4 %19 / 14250.0 %5 / 10
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_lib/src/profiler +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_managers/include/transform_manager +
48.2%48.2%
+ 		48.2 %192 / 39868.4 %13 / 19
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.1%53.1%
+ 		53.1 %405 / 76369.5 %66 / 95
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
mrs_uav_managers/src/control_manager +
69.8%69.8%
+ 		69.8 %2594 / 371487.0 %107 / 123
mrs_uav_managers/src/estimation_manager +
69.9%69.9%
+ 		69.9 %437 / 62585.1 %40 / 47
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.7%71.7%
+ 		71.7 %132 / 18450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_managers/src +
72.6%72.6%
+ 		72.6 %1247 / 171887.7 %64 / 73
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98562.5 %846 / 1354
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_controllers/src +
80.9%80.9%
+ 		80.9 %1759 / 217487.9 %58 / 66
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44659.5 %830 / 1394
mrs_uav_managers/src/transform_manager +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
mrs_uav_trackers/src/mpc_tracker +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_uav_autostart/src +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index.html b/index.html new file mode 100644 index 0000000000..9e8f03629a --- /dev/null +++ b/index.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:143212139466.9 %
Date:2024-06-08 22:13:12Functions:2641417363.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98562.5 %846 / 1354
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44659.5 %830 / 1394
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_lib/src/profiler +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
13.4%13.4%
+ 		13.4 %19 / 14250.0 %5 / 10
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_autostart/src +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_controllers/src +
80.9%80.9%
+ 		80.9 %1759 / 217487.9 %58 / 66
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_uav_managers/include/transform_manager +
48.2%48.2%
+ 		48.2 %192 / 39868.4 %13 / 19
mrs_uav_managers/src +
72.6%72.6%
+ 		72.6 %1247 / 171887.7 %64 / 73
mrs_uav_managers/src/control_manager +
69.8%69.8%
+ 		69.8 %2594 / 371487.0 %107 / 123
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_managers/src/estimation_manager +
69.9%69.9%
+ 		69.9 %437 / 62585.1 %40 / 47
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_managers/src/transform_manager +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.1%53.1%
+ 		53.1 %405 / 76369.5 %66 / 95
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
71.7%71.7%
+ 		71.7 %132 / 18450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_trackers/src/mpc_tracker +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html new file mode 100644 index 0000000000..46a75a37fb --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-06-08 22:13:12Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
auto mrs_lib::AttitudeConverter::get<0ul>()1
auto mrs_lib::AttitudeConverter::get<1ul>()1
auto mrs_lib::AttitudeConverter::get<2ul>()1
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen9AngleAxisIT_EEIdEEv1
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv103095
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)317055
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)627960
+
+
+ + + +
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..670a85a17d --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-06-08 22:13:12Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)317055
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>)627960
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv103095
_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..d476b09b2a --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html @@ -0,0 +1,617 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-06-08 22:13:12Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::sdegrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::sradians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::value() const0
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::convert<mrs_lib::geometry::radians>() const1
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::value() const4
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>&&)6
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::value() const10002
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10004
mrs_lib::geometry::operator-(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10009
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10010
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10374
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(double)20000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::wrap(double)30000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)40000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::value() const40002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)432790
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)529496
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)663160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)663160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)663160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)844296
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)886979
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>)973205
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)1346348
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1976102
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1998534
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)3474529
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)3488322
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>)4161726
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)8423455
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)9216762
+
+
+ + + +
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..0d9b01d7f4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-06-08 22:13:12Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)973205
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)529496
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>)432790
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10374
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1998534
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)844296
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)886979
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1976102
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)663160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)663160
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>)4161726
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)3488322
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10010
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)9216762
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)663160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)3474529
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&)1346348
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)8423455
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..2af8870d68 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-06-08 22:13:12Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines the cyclic class for calculations with cyclic quantities.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef CYCLIC_H
+       8             : #define CYCLIC_H
+       9             : 
+      10             : #include <cmath>
+      11             : #include <ostream>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             :   namespace geometry
+      16             :   {
+      17             :     /**
+      18             :      * \brief Implementation of the a general cyclic value (such as angles in radians/degrees etc).
+      19             :      *
+      20             :      * This class implements a periodical value with a period of \f$ r \in {\rm I\!R} \f$ so that a general number \f$ v \in {\rm I\!R} \f$ represents the same
+      21             :      * value as \f$ v + kr,~k \in {\rm I\!N} \f$. For the purposes of calculations in this class, \f$ v \f$ is confined to a half-open interval \f$ v \in
+      22             :      * [~m,~s~[ \f$, where \f$ m \f$ is the minimum of this interval and \f$ s \f$ is its supremum, and \f$ s = m + r \f$ holds. This approach enables
+      23             :      * representing \f$ v \f$ in different intervals on the real numbers axis; eg. angle in radians may be represented within the interval \f$ v \in
+      24             :      * [~-\pi,~\pi~[ \f$ or \f$ v \in [~0,~2\pi~[ \f$, according to the needs of the specific application. The period \f$ r \f$ is called \p range for the
+      25             :      * purposes of this class, as it represents range of the interval of valid ("wrapped") values of \f$ v \f$.
+      26             :      *
+      27             :      * This class may be used as an object or its static methods may be used on regular floating-point types, avoiding any object-related overheads (see
+      28             :      * example). Specializations for the most common cyclic values are provided and a new specialization may be easily created simply by inheriting from this
+      29             :      * class and specifying a different minimum and supremum values.
+      30             :      *
+      31             :      * Implementation inspired by: https://www.codeproject.com/Articles/190833/Circular-Values-Math-and-Statistics-with-Cplusplus
+      32             :      *
+      33             :      * \parblock
+      34             :      * \note For a better intuitive understanding of the used functions, the term **walk** is sometimes used in the function explanations.
+      35             :      * You can imagine walking along the circle from one angle to another (represented by the circular quantities).
+      36             :      * The walk may be the shortest - then you're walking in such a manner that you reach the other point in the least of steps.
+      37             :      * The walk may also be oriented - then you're walking in a specific direction (ie. according to the increasing/decreasing angle).
+      38             :      * \endparblock
+      39             :      *
+      40             :      * \parblock
+      41             :      * \note The terms **circular quantity** and **value** are used in the function explanations.
+      42             :      * A circular quantity is eg. an angle and the same quantity may be represented using different values: the same angle is represented by the values of 100
+      43             :      * degrees and 460 degrees. \endparblock
+      44             :      *
+      45             :      * \tparam flt floating data type to be used by this class.
+      46             :      */
+      47             :     template <typename flt, class spec>
+      48             :     struct cyclic
+      49             :     {
+      50             :       /*!
+      51             :        * \brief Default constructor.
+      52             :        *
+      53             :        * Sets the value to the minimum.
+      54             :        */
+      55           0 :       cyclic() : val(minimum){};
+      56             :       /*!
+      57             :        * \brief Constructor overload.
+      58             :        *
+      59             :        * \param val initialization value (will be wrapped).
+      60             :        */
+      61    10439562 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67     2233327 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73      844296 :       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    11275301 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117    11275301 :         if (val >= minimum)
+     118             :         {
+     119    10481022 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120    10249995 :             return val;
+     121      231027 :           else if (val < supremum + range)
+     122       67166 :             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      794284 :           if (val >= minimum - range)
+     126      631455 :             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      326697 :         const flt rem = std::fmod(val - minimum, range);
+     131      326697 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326475 :         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     3504530 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159     3504530 :         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     4037818 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195     4037818 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198     5154931 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200     5154931 :         const flt d = minuend.val - subtrahend.val;
+     201     5154931 :         if (d < -half_range)
+     202       42944 :           return d + range;
+     203     5111987 :         if (d >= half_range)
+     204       29063 :           return d - range;
+     205     5082925 :         return d;
+     206             :       }
+     207             : 
+     208             :       /*!
+     209             :        * \brief Returns the distance between the two circular values.
+     210             :        *
+     211             :        * The distance may also be interpreted as length of the shortest walk between the two values.
+     212             :        *
+     213             :        * \param from  the first circular quantity.
+     214             :        * \param to    the second circular quantity.
+     215             :        * \returns     distance of the two circular quantities.
+     216             :        *
+     217             :        * \note The order of the parameters doesn't matter.
+     218             :        */
+     219       20376 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20376 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224      442800 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226      442800 :         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      664058 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244      664058 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247      664058 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249      664058 :         const flt dang = diff(to, from);
+     250      664058 :         const flt intp = from.val + coeff * dang;
+     251      664058 :         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      664058 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266      664058 :         return wrap(interpUnwrapped(from, to, coeff));
+     267             :       }
+     268             : 
+     269           0 :       static flt interp(const cyclic from, const cyclic to, const flt coeff)
+     270             :       {
+     271           0 :         return wrap(interpUnwrapped(from, to, coeff));
+     272             :       }
+     273             : 
+     274             :       /*!
+     275             :        * \brief Interpolation between two circular quantities in the positive direction without wrapping of the result.
+     276             :        *
+     277             :        * Interpolates the two values in the positive direction from the first parameter to the second by the coefficient.
+     278             :        * This function doesn't wrap the returned value.
+     279             :        *
+     280             :        * \param from  the first circular quantity.
+     281             :        * \param to    the second circular quantity.
+     282             :        * \param coeff the interpolation coefficient.
+     283             :        * \returns     interpolation of the two circular quantities using the coefficient.
+     284             :        *
+     285             :        * \warning Note that the returned value may be outside the valid interval of wrapped values, specified by the \p minimum and \p supremum parameters of
+     286             :        * this class.
+     287             :        */
+     288           0 :       static flt pinterpUnwrapped(const flt from, const flt to, const flt coeff)
+     289             :       {
+     290           0 :         return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
+     291             :       }
+     292             : 
+     293           0 :       static flt pinterpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     294             :       {
+     295           0 :         const flt dang = pdist(to, from);
+     296           0 :         const flt intp = from.val + coeff * dang;
+     297           0 :         return intp;
+     298             :       }
+     299             : 
+     300             :       /*!
+     301             :        * \brief Interpolation between two circular quantities in the positive direction.
+     302             :        *
+     303             :        * Interpolates the two values in the positive direction from the first parameter to the second by the coefficient.
+     304             :        * This function wraps the returned value so that it is in the interval of valid values.
+     305             :        *
+     306             :        * \param from  the first circular quantity.
+     307             :        * \param to    the second circular quantity.
+     308             :        * \param coeff the interpolation coefficient.
+     309             :        * \returns     interpolation of the two circular quantities using the coefficient, wrapped to the interval of valid values.
+     310             :        */
+     311           0 :       static flt pinterp(const flt from, const flt to, const flt coeff)
+     312             :       {
+     313           0 :         return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
+     314             :       }
+     315             : 
+     316           0 :       static flt pinterp(const cyclic from, const cyclic to, const flt coeff)
+     317             :       {
+     318           0 :         return wrap(pinterpUnwrapped(from, to, coeff));
+     319             :       }
+     320             : 
+     321             :       /*!
+     322             :        * \brief Conversion between two different circular quantities.
+     323             :        *
+     324             :        * This function converts its parameter, interpreted as the circular quantity, represented by this class, to the \p other_t type of circular quantity.
+     325             :        *
+     326             :        * \param what       the circular quantity to be converted.
+     327             :        * \returns          the circular quantity converted to the range of \p other_t.
+     328             :        * \tparam other_t   type of the circular quantity to be converted to.
+     329             :        *
+     330             :        * \warning For the purposes of this function, it is assumed that the range of one type corresponds to the whole range of the other type and zeros of both
+     331             :        * types correspond to each other (such as when converting eg. degrees to radians).
+     332             :        */
+     333             :       template <class other_t>
+     334           1 :       static other_t convert(const cyclic& what)
+     335             :       {
+     336           1 :         return other_t(what.val / range * other_t::range);
+     337             :       }
+     338             : 
+     339             :       /*!
+     340             :        * \brief Conversion between two different circular quantities.
+     341             :        *
+     342             :        * This method returns the circular quantity, represented by this object, converted to the \p other_t type of circular quantity.
+     343             :        *
+     344             :        * \returns          the circular quantity converted to the range of \p other_t.
+     345             :        * \tparam other_t   type of the circular quantity to be converted to.
+     346             :        *
+     347             :        * \warning For the purposes of this function, it is assumed that the range of one type corresponds to the whole range of the other type and zeros of both
+     348             :        * types correspond to each other (such as when converting eg. degrees to radians).
+     349             :        */
+     350             :       template <class other_t>
+     351           1 :       other_t convert() const
+     352             :       {
+     353           1 :         return other_t(val / range * other_t::range);
+     354             :       }
+     355             : 
+     356             :       // | ------------------------ Operators ----------------------- |
+     357             :       /*!
+     358             :        * \brief Assignment operator.
+     359             :        *
+     360             :        * \param nval value to be assigned (will be wrapped).
+     361             :        * \return     reference to self.
+     362             :        */
+     363           0 :       cyclic& operator=(const flt nval)
+     364             :       {
+     365           0 :         val = wrap(nval);
+     366           0 :         return *this;
+     367             :       };
+     368             :       /*!
+     369             :        * \brief Assignment operator.
+     370             :        *
+     371             :        * \param other value to be assigned.
+     372             :        * \return      reference to self.
+     373             :        */
+     374           0 :       cyclic& operator=(const cyclic& other)
+     375             :       {
+     376           0 :         val = other.val;
+     377           0 :         return *this;
+     378             :       };
+     379             :       /*!
+     380             :        * \brief Move operator.
+     381             :        *
+     382             :        * \param other value to be assigned.
+     383             :        * \return      reference to self.
+     384             :        */
+     385           6 :       cyclic& operator=(cyclic&& other)
+     386             :       {
+     387           6 :         val = other.val;
+     388           6 :         return *this;
+     389             :       };
+     390             : 
+     391             :       /*!
+     392             :        * \brief Addition compound operator.
+     393             :        *
+     394             :        * \param other value to be added.
+     395             :        * \return      reference to self.
+     396             :        */
+     397           1 :       cyclic& operator+=(const cyclic& other)
+     398             :       {
+     399           1 :         val = wrap(val + other.val);
+     400           1 :         return *this;
+     401             :       };
+     402             : 
+     403             :       /*!
+     404             :        * \brief Subtraction compound operator.
+     405             :        *
+     406             :        * \param other value to be subtracted.
+     407             :        * \return      reference to self.
+     408             :        */
+     409           1 :       cyclic& operator-=(const cyclic& other)
+     410             :       {
+     411           1 :         val = diff(val, other.val);
+     412           1 :         return *this;
+     413             :       };
+     414             : 
+     415             :       /*!
+     416             :        * \brief Addition operator.
+     417             :        *
+     418             :        * \param lhs left-hand-side.
+     419             :        * \param rhs right-hand-side.
+     420             :        * \return    the result of adding the two angles.
+     421             :        */
+     422       50004 :       friend spec operator+(const cyclic& lhs, const cyclic& rhs)
+     423             :       {
+     424       50004 :         return wrap(lhs.val + rhs.val);
+     425             :       }
+     426             : 
+     427             :       /*!
+     428             :        * \brief Subtraction operator (uses the diff() method).
+     429             :        *
+     430             :        * \param lhs left-hand-side.
+     431             :        * \param rhs right-hand-side.
+     432             :        * \return    the result of subtracting rhs from lhs.
+     433             :        */
+     434       10009 :       friend flt operator-(const cyclic& lhs, const cyclic& rhs)
+     435             :       {
+     436       10009 :         return diff(lhs, rhs);
+     437             :       }
+     438             : 
+     439             :       protected:
+     440             :         flt val;
+     441             :       };
+     442             : 
+     443             :     /*!
+     444             :      * \brief Implementation of the comparison operation between two angles.
+     445             :      *
+     446             :      * An angle is considered to be smaller than another angle if it is shorter - closer to zero.
+     447             :      *
+     448             :      * \param lhs left-hand-side.
+     449             :      * \param rhs right-hand-side.
+     450             :      * \return    true iff the shortest unsigned walk from lhs to 0 is less than from rhs to 0.
+     451             :      */
+     452             :     template <typename flt, class spec>
+     453           4 :     bool operator<(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
+     454             :     {
+     455           4 :       return cyclic<flt, spec>::dist(lhs, 0) < cyclic<flt, spec>::dist(rhs, 0);
+     456             :     }
+     457             : 
+     458             :     /*!
+     459             :      * \brief Implementation of the comparison operation between two angles.
+     460             :      *
+     461             :      * An angle is considered to be larger than another angle if it is longer - further from zero.
+     462             :      *
+     463             :      * \param lhs left-hand-side.
+     464             :      * \param rhs right-hand-side.
+     465             :      * \return    true iff the shortest unsigned walk from lhs to 0 is more than from rhs to 0.
+     466             :      */
+     467             :     template <typename flt, class spec>
+     468           4 :     bool operator>(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
+     469             :     {
+     470           4 :       return cyclic<flt, spec>::dist(lhs, 0) > cyclic<flt, spec>::dist(rhs, 0);
+     471             :     }
+     472             : 
+     473             :     /*!
+     474             :      * \brief Implementation of the stream output operator.
+     475             :      *
+     476             :      * \param out the stream to write the angle to.
+     477             :      * \param ang the angle to be written.
+     478             :      * \return    a reference to the stream.
+     479             :      */
+     480             :     template <typename flt, class spec>
+     481             :     std::ostream& operator<<(std::ostream &out, const cyclic<flt, spec>& ang)
+     482             :     {
+     483             :       return (out << ang.value());
+     484             :     }
+     485             : 
+     486             :     /*!
+     487             :      * \brief Convenience specialization of the cyclic class for unsigned radians (from $0$ to $2\pi$).
+     488             :      */
+     489             :     struct radians : public cyclic<double, radians>
+     490             :     {
+     491             :       using cyclic<double, radians>::cyclic;  // necessary to inherit constructors
+     492             :       static constexpr double minimum = 0;
+     493             :       static constexpr double supremum = 2 * M_PI;
+     494             :     };
+     495             : 
+     496             :     /*!
+     497             :      * \brief Convenience specialization of the cyclic class for signed radians (from $-\pi$ to $\pi$).
+     498             :      */
+     499             :     struct sradians : public cyclic<double, sradians>
+     500             :     {
+     501             :       using cyclic<double, sradians>::cyclic;  // necessary to inherit constructors
+     502             :       static constexpr double minimum = -M_PI;
+     503             :       static constexpr double supremum = M_PI;
+     504             :     };
+     505             : 
+     506             :     /*!
+     507             :      * \brief Convenience specialization of the cyclic class for unsigned degrees (from $0$ to $360$).
+     508             :      */
+     509             :     struct degrees : public cyclic<double, degrees>
+     510             :     {
+     511             :       using cyclic<double, degrees>::cyclic;  // necessary to inherit constructors
+     512             :       static constexpr double minimum = 0;
+     513             :       static constexpr double supremum = 360;
+     514             :     };
+     515             : 
+     516             :     /*!
+     517             :      * \brief Convenience specialization of the cyclic class for signed degrees (from $-180$ to $180$).
+     518             :      */
+     519             :     struct sdegrees : public cyclic<double, sdegrees>
+     520             :     {
+     521             :       using cyclic<double, sdegrees>::cyclic;  // necessary to inherit constructors
+     522             :       static constexpr double minimum = -180;
+     523             :       static constexpr double supremum = 180;
+     524             :     };
+     525             :   }  // namespace geometry
+     526             : }  // namespace mrs_lib
+     527             : 
+     528             : #endif  // CYCLIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html new file mode 100644 index 0000000000..1ad3dfadb9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html @@ -0,0 +1,152 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
cyclic.h +
69.0%69.0%
+ 		69.0 %60 / 8741.0 %48 / 117
misc.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html b/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html new file mode 100644 index 0000000000..725822a423 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-06-08 22:13:12Functions: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&)146781
+
+
+ + + +
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..c595a4d533 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-06-08 22:13:12Functions: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&)146781
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..d9df630b7b --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html @@ -0,0 +1,408 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-06-08 22:13:12Functions: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      146787 :     double headingFromRot(const T& rot)
+      58             :     {
+      59      146787 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60      293574 :       return std::atan2(rot_vec.y(), rot_vec.x());
+      61             :     }
+      62             : 
+      63             :     //}
+      64             : 
+      65             :     /* angleBetween() //{ */
+      66             : 
+      67             :     /*!
+      68             :      * \brief Returns the angle between two vectors, taking orientation into account.
+      69             :      *
+      70             :      * This implementation uses \p atan2 instead of just \p acos and thus it properly
+      71             :      * takes into account orientation of the vectors, returning angle in all four quadrants.
+      72             :      *
+      73             :      * \param a vector from which the angle will be measured.
+      74             :      * \param b vector to which the angle will be measured.
+      75             :      *
+      76             :      * \returns    angle from \p a to \p b.
+      77             :      *
+      78             :      */
+      79             :     double angleBetween(const vec2_t& a, const vec2_t& b);
+      80             : 
+      81             :     /*!
+      82             :      * \brief Returns the angle between two vectors, taking orientation into account.
+      83             :      *
+      84             :      * This implementation uses \p atan2 instead of just \p acos and thus it properly
+      85             :      * takes into account orientation of the vectors, returning angle in all four quadrants.
+      86             :      *
+      87             :      * \param a vector from which the angle will be measured.
+      88             :      * \param b vector to which the angle will be measured.
+      89             :      *
+      90             :      * \returns    angle from \p a to \p b.
+      91             :      *
+      92             :      */
+      93             :     double angleBetween(const vec3_t& a, const vec3_t& b);
+      94             : 
+      95             :     //}
+      96             : 
+      97             :     /* angleaxisBetween() //{ */
+      98             : 
+      99             :     /*!
+     100             :      * \brief Returns the rotation between two vectors, represented as angle-axis.
+     101             :      *
+     102             :      * To avoid singularities, a \p tolerance parameter is used:
+     103             :      * * If the absolute angle between the two vectors is less than \p tolerance, a zero rotation is returned.
+     104             :      * * If the angle between the two vectors is closer to \f$ \pi \f$ than \p tolerance, a \f$ \pi \f$ rotation is returned.
+     105             :      *
+     106             :      * \param a vector from which the rotation starts.
+     107             :      * \param b vector at which the rotation ends.
+     108             :      *
+     109             :      * \returns    rotation from \p a to \p b.
+     110             :      *
+     111             :      */
+     112             :     anax_t angleaxisBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     113             : 
+     114             :     //}
+     115             : 
+     116             :     /* quaternionBetween() //{ */
+     117             : 
+     118             :     /*!
+     119             :      * \brief Returns the rotation between two vectors, represented as a quaternion.
+     120             :      *
+     121             :      * Works the same as the angleaxisBetween() function (in fact it is used in the implementation).
+     122             :      *
+     123             :      * \param a vector from which the rotation starts.
+     124             :      * \param b vector at which the rotation ends.
+     125             :      *
+     126             :      * \returns    rotation from \p a to \p b.
+     127             :      *
+     128             :      */
+     129             :     quat_t quaternionBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     130             : 
+     131             :     //}
+     132             : 
+     133             :     /* rotationBetween() //{ */
+     134             : 
+     135             :     /*!
+     136             :      * \brief Returns the rotation between two vectors, represented as a rotation matrix.
+     137             :      *
+     138             :      * Works the same as the angleaxisBetween() function (in fact it is used in the implementation).
+     139             :      *
+     140             :      * \param a vector from which the rotation starts.
+     141             :      * \param b vector at which the rotation ends.
+     142             :      *
+     143             :      * \returns    rotation from \p a to \p b.
+     144             :      *
+     145             :      */
+     146             :     Eigen::Matrix3d rotationBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     147             : 
+     148             :     //}
+     149             : 
+     150             :     /* haversin() //{ */
+     151             : 
+     152             :     /**
+     153             :      * @brief computes the haversine (half of versine) for a given angle
+     154             :      *
+     155             :      * @param angle angle in radians
+     156             :      *
+     157             :      * @return
+     158             :      */
+     159             :     double haversin(const double angle);
+     160             : 
+     161             :     //}
+     162             : 
+     163             :     /* invHaversin() //{ */
+     164             : 
+     165             :     /**
+     166             :      * @brief computes the inverse haversine angle for a given value
+     167             :      *
+     168             :      * @param value
+     169             :      *
+     170             :      * @return angle in radians
+     171             :      */
+     172             :     double invHaversin(const double value);
+     173             : 
+     174             :     //}
+     175             : 
+     176             :     /* solidAngle //{ */
+     177             : 
+     178             :     /**
+     179             :      * @brief computes the solid angle for a spherical surface corresponding to a 'triangle' with edge lengths a, b, c
+     180             :      *
+     181             :      * @param a
+     182             :      * @param b
+     183             :      * @param c
+     184             :      *
+     185             :      * @return solid angle in steradians
+     186             :      */
+     187             :     double solidAngle(double a, double b, double c);
+     188             : 
+     189             :     //}
+     190             : 
+     191             :     /* quaternionFromEuler() //{ */
+     192             : 
+     193             :     /**
+     194             :      * @brief create a quaternion from 3 provided Euler angles
+     195             :      *
+     196             :      * @param x Euler angle in radians
+     197             :      * @param y Euler angle in radians
+     198             :      * @param z Euler angle in radians
+     199             :      *
+     200             :      * @return quaternion
+     201             :      */
+     202             :     quat_t quaternionFromEuler(double x, double y, double z);
+     203             : 
+     204             :     /**
+     205             :      * @brief create a quaternion from Euler angles provided as a vector
+     206             :      *
+     207             :      * @param euler components of the rotation provided as vector of Euler angles
+     208             :      *
+     209             :      * @return quaternion
+     210             :      */
+     211             :     quat_t quaternionFromEuler(const Eigen::Vector3d& euler);
+     212             : 
+     213             :     //}
+     214             : 
+     215             :     /**
+     216             :      * @brief create a quaternion from heading
+     217             :      *
+     218             :      * Heading is defined as the angle of a [1,0,0] vector rotated by the rotation and projected to the XY plane from the X axis.
+     219             :      *
+     220             :      * @param heading the heading angle.
+     221             :      *
+     222             :      * @return the quaternion corresponding to the heading rotation.
+     223             :      */
+     224             :     quat_t quaternionFromHeading(const double heading);
+     225             : 
+     226             :     //}
+     227             : 
+     228             :     // | ----------------- Miscellaneous functions ---------------- |
+     229             : 
+     230             :     /* 2D cross() //{ */
+     231             : 
+     232             :     /*!
+     233             :      * \brief Implementation of cross product for 2D vectors.
+     234             :      *
+     235             :      * Useful e.g. for finding the sine of an angle between two 2D vectors.
+     236             :      *
+     237             :      * \param a first vector of the cross product.
+     238             :      * \param b second vector of the cross product.
+     239             :      *
+     240             :      * \returns    \f$ a \times b \f$ (sine of the angle from \p a to \p b).
+     241             :      *
+     242             :      */
+     243             :     double cross(const vec2_t& a, const vec2_t b);
+     244             : 
+     245             :     //}
+     246             : 
+     247             :     /* vector distance //{ */
+     248             : 
+     249             :     /**
+     250             :      * @brief distnace between two 2D Eigen vectors
+     251             :      *
+     252             :      * @param a
+     253             :      * @param b
+     254             :      *
+     255             :      * @return Euclidean distance
+     256             :      */
+     257             :     double dist(const vec2_t& a, const vec2_t& b);
+     258             : 
+     259             :     /**
+     260             :      * @brief distnace between two 3D Eigen vectors
+     261             :      *
+     262             :      * @param a
+     263             :      * @param b
+     264             :      *
+     265             :      * @return Euclidean distance
+     266             :      */
+     267             :     double dist(const vec3_t& a, const vec3_t& b);
+     268             : 
+     269             :     //}
+     270             : 
+     271             :     /* triangleArea() //{ */
+     272             : 
+     273             :     /**
+     274             :      * @brief uses Heron's formula to compute area of a given triangle using side lengths
+     275             :      *
+     276             :      * @param a length of side1
+     277             :      * @param b length of side2
+     278             :      * @param c length of side3
+     279             :      *
+     280             :      * @return area in units squared
+     281             :      */
+     282             :     double triangleArea(const double a, const double b, const double c);
+     283             : 
+     284             :     //}
+     285             : 
+     286             :     /* sphericalTriangleArea //{ */
+     287             : 
+     288             :     /**
+     289             :      * @brief compute the area of a 'triangle' drawn on a spherical surface
+     290             :      *
+     291             :      * @param a length of side1
+     292             :      * @param b length of side2
+     293             :      * @param c length of side3
+     294             :      *
+     295             :      * @return area in units squared
+     296             :      */
+     297             :     double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
+     298             : 
+     299             :     //}
+     300             : 
+     301             :     /* rotateCovariance() //{ */
+     302             :     
+     303             :     /*!
+     304             :      * \brief Returns the covariance rotated using the specified rotation.
+     305             :      *
+     306             :      * \param cov the covariance to be rotated.
+     307             :      * \param rot the rotation use.
+     308             :      *
+     309             :      * \returns a new matrix object containing the rotated covariance.
+     310             :      *
+     311             :      */
+     312             :     template <typename T>
+     313             :     mat3_t rotateCovariance(const mat3_t& cov, const T& rot)
+     314             :     {
+     315             :       const mat3_t matrot(rot);
+     316             :       return matrot*cov*matrot.transpose();
+     317             :     }
+     318             :     
+     319             :     //}
+     320             : 
+     321             :   }  // namespace geometry
+     322             : }  // namespace mrs_lib
+     323             : 
+     324             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html new file mode 100644 index 0000000000..5f250b8ef8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParam<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParamImpl<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParam<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParamImpl<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&) const1915
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> >&) const1915
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> > > >&) const3751
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> > > >&) const3751
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5423
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5423
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> >&) const18078
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> >&) const18078
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const21418
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const21418
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const43796
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const43796
+
+
+ + + +
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..afdbac04c3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-06-08 22:13:12Functions:1616100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const18078
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> > > >&) const3751
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> >&) const1915
bool mrs_lib::ParamProvider::getParamImpl<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParamImpl<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const21418
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const43796
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5423
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> >&) const18078
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> > > >&) const3751
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> >&) const1915
bool mrs_lib::ParamProvider::getParam<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const10
bool mrs_lib::ParamProvider::getParam<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const5
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const21418
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const43796
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5423
+
+
+ + + +
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..dc1eacec0e --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-06-08 22:13:12Functions:1616100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > > const&)1
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > > const&)2
mrs_lib::PublisherHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)31
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)94
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&)107
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)107
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&)107
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&)107
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&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)107
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&)107
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&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)107
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&)107
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&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)107
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&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)107
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&)107
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)107
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&)108
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&)108
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<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&)112
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)112
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&)112
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)215
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&)220
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)220
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&)220
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&)310
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)310
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&)310
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&)422
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)422
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&)422
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&)428
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)428
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&)428
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)428
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&)428
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&)428
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)435
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)435
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)435
mrs_lib::PublisherHandler<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&)524
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&)524
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
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&)621
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)621
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&)621
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&)642
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)642
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&)642
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&)812
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)812
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&)812
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1689
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1689
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1801
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1801
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1803
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1803
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2176
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2176
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2233
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2233
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2289
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2289
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7076
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7076
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9534
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9534
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11694
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11694
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)12796
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)12796
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)15628
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)15628
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)18436
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)18436
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)18869
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)18869
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)23466
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)23466
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)23466
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)23466
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)35660
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)35660
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)94724
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)94724
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)109596
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)109596
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)146218
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)146218
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)152692
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)173964
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)173964
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)205384
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)205385
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)246411
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)246411
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)368179
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)368184
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)376054
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)376056
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)508434
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)508969
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)714258
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)714294
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)790711
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)790719
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1442159
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1442359
+
+
+ + + +
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..d8ad85f5d7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,904 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-06-08 22:13:12Functions:20120697.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)173964
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&)428
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)428
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1689
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&)107
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)205384
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&)112
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)112
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)35660
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&)642
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)642
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11694
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)94724
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&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)376054
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&)621
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)621
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)109596
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&)107
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)714258
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&)422
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)422
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7076
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > > const&)2
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)435
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > > const&)1
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2289
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&)107
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)15628
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&)107
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1442359
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&)812
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)812
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)508969
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&)310
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)310
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2176
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&)108
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)215
mrs_lib::PublisherHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)146218
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)18436
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&)107
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)23466
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&)107
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2233
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&)107
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1801
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&)107
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)94
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)18869
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
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&)107
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)214
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1803
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&)107
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)107
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)368184
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&)220
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)220
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)790719
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)152692
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&)524
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)435
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9534
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)31
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)12796
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&)107
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)107
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&)23466
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&)107
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)107
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)246411
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&)428
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)428
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)173964
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&)428
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1689
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&)107
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)205385
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&)112
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)35660
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&)642
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11694
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)94724
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)376056
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&)621
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)109596
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)714294
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&)422
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7076
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)435
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2289
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)15628
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1442159
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&)812
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)508434
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&)310
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2176
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&)108
mrs_lib::PublisherHandler_impl<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::publish(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)146218
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)18436
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)23466
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2233
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1801
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)94
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)18869
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1803
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&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<std::allocator<void> > const&)5
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)107
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)368179
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&)220
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)790711
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&)524
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9534
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)31
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)12796
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&)107
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&)23466
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&)107
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)246411
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&)428
+
+
+ + + +
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..f6ba64c206 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html @@ -0,0 +1,332 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-06-08 22:13:12Functions:20120697.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride> const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv> const&)2
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&, int const&, double const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::asyncRun()2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)94
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)107
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)110
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)110
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)214
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)214
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)214
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)264
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)264
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)353
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)353
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)419
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)419
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)644
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)644
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)644
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1065
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)1065
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1172
+
+
+ + + +
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..9676963ea1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-06-08 22:13:12Functions:4747100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv> const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride> const&)1
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)94
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)110
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)419
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)214
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)214
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)353
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)644
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)644
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)264
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&)1065
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)1065
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::call(mrs_msgs::Float64Srv&)11
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::call(mrs_msgs::ConstraintsOverride&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)94
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)110
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)20
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)419
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)214
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)353
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)644
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)264
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&)1172
+
+
+ + + +
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..405e302b56 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html @@ -0,0 +1,403 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-06-08 22:13:12Functions:4747100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef SERVICE_CLIENT_HANDLER_HPP
+       2             : #define SERVICE_CLIENT_HANDLER_HPP
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             : 
+       7             : // --------------------------------------------------------------
+       8             : // |                  ServiceClientHandler_impl                 |
+       9             : // --------------------------------------------------------------
+      10             : 
+      11             : /* ServiceClientHandler_impl(void) //{ */
+      12             : 
+      13             : template <class ServiceType>
+      14             : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(void) : service_initialized_(false) {
+      15             : }
+      16             : 
+      17             : //}
+      18             : 
+      19             : /* ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) //{ */
+      20             : 
+      21             : template <class ServiceType>
+      22        2356 : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) {
+      23             : 
+      24             :   {
+      25        2356 :     std::scoped_lock lock(mutex_service_client_);
+      26             : 
+      27        2356 :     service_client_ = nh.serviceClient<ServiceType>(address);
+      28             :   }
+      29             : 
+      30        2356 :   _address_       = address;
+      31        2356 :   async_attempts_ = 1;
+      32             : 
+      33             :   /* thread_oneshot_ = std::make_shared<std::thread>(std::thread(&ServiceClientHandler_impl::threadOneshot, this, true, false)); */
+      34             : 
+      35        2356 :   service_initialized_ = true;
+      36        2356 : }
+      37             : 
+      38             : //}
+      39             : 
+      40             : /* call(ServiceType& srv) //{ */
+      41             : 
+      42             : template <class ServiceType>
+      43        1273 : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv) {
+      44             : 
+      45        1273 :   if (!service_initialized_) {
+      46           0 :     return false;
+      47             :   }
+      48             : 
+      49        1273 :   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        2249 : ServiceClientHandler<ServiceType>& ServiceClientHandler<ServiceType>::operator=(const ServiceClientHandler<ServiceType>& other) {
+     205             : 
+     206        2249 :   if (this == &other) {
+     207           0 :     return *this;
+     208             :   }
+     209             : 
+     210        2249 :   if (other.impl_) {
+     211        2249 :     this->impl_ = other.impl_;
+     212             :   }
+     213             : 
+     214        2249 :   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        2249 : ServiceClientHandler<ServiceType>::ServiceClientHandler(ros::NodeHandle& nh, const std::string& address) {
+     235             : 
+     236        2249 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     237        2249 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : /* initialize(ros::NodeHandle& nh, const std::string& address) //{ */
+     242             : 
+     243             : template <class ServiceType>
+     244         107 : void ServiceClientHandler<ServiceType>::initialize(ros::NodeHandle& nh, const std::string& address) {
+     245             : 
+     246         107 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     247         107 : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* call(ServiceType& srv) //{ */
+     252             : 
+     253             : template <class ServiceType>
+     254        1273 : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv) {
+     255             : 
+     256        1273 :   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..f955345b3a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html @@ -0,0 +1,4256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-06-08 22:13:12Functions:495104447.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::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::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::stop()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const6
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)8
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const9
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() const22
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()95
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const95
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const96
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()97
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const97
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const97
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const108
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const108
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&)109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const110
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().2112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const112
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()113
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const122
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const133
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()207
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&)207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()207
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&)207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const207
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const211
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const211
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()214
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&)214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()214
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&)214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()214
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&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()214
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&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()214
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()214
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const214
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()245
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&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()245
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&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const245
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const259
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const259
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()303
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()303
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const303
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const303
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()321
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&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()321
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&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const321
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()329
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&)329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()329
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&)329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const330
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)338
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)338
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()340
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&)340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()340
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&)340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const340
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()352
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&)352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()352
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&)352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2352
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()363
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()363
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const363
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const363
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()417
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()417
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const417
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()426
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&)426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()426
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&)426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const426
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()428
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&)428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()428
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&)428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const428
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const459
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const460
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)573
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)573
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const581
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()640
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&)640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()640
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&)640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2640
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const710
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const792
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const863
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1801
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1801
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1803
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1803
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2151
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2151
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2655
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2655
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3108
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3108
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()4557
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()4557
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4557
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const4557
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5568
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5580
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8224
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8241
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const14585
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const14585
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()14588
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()14588
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)15614
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)15614
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17662
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const17662
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18480
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const18480
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const18701
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18702
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18709
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const18709
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)18910
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)18910
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()19453
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const19453
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const19453
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()19454
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)24316
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)24316
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const37905
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const37905
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const39907
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()51212
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const51217
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const51217
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()51218
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)55648
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)55648
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const57883
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const57883
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const71950
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const71957
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()72512
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const72526
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()72534
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const72535
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)77516
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)77755
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)80403
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)80675
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const85343
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const85364
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const117754
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const117754
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)146122
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)146122
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()147771
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const147781
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()147784
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const147784
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const147833
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const147859
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()179465
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()179465
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const179465
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const179465
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const179470
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const179470
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const197052
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()197054
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()197055
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const197055
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)202349
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)202351
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)203398
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)203410
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)207426
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)207447
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)218783
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)218824
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const227722
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const227722
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()246331
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()246332
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const246333
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const246333
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const265249
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const265252
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)304285
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)304907
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)341156
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)341260
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)345004
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)345060
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)381214
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)383324
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const407896
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const407914
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const428484
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()428558
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()428577
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const428578
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const453641
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const453657
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)510030
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)510238
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)540144
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)540241
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const560160
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const560367
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const572552
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const572553
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)637588
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)637825
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()764850
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const764862
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()764866
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const764891
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const979663
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const979691
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)1007644
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)1007774
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1232418
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1232645
+
+
+ + + +
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..710d2a57e8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,4256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-06-08 22:13:12Functions:495104447.4 %
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&)304907
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()426
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&)426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2426
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&)304285
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()426
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&)426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2426
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)55648
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)55648
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()214
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&)214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2214
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()214
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&)214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2214
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)341260
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()14588
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&)207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2207
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&)341156
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()207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()14588
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&)207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2207
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&)202351
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()4557
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&)109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)202349
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()113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()4557
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&)109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)637825
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()197054
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&)340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2340
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&)637588
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()340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()197055
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&)340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2340
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1232418
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()764850
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&)640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2640
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&)1232645
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)8
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()764866
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&)640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2640
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)1007774
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()303
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&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2245
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&)1007644
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()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()303
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&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)207426
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()179465
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
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&)207447
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()218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()179465
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)383324
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()428558
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2417
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&)381214
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()417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()428577
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2417
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)345060
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()51212
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&)428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2428
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&)345004
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()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()51218
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&)428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2428
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)218824
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2214
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&)218783
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()214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2214
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)203410
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2112
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&)203398
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()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().2112
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)24316
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()710
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
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&)24316
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()107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()710
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()363
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()363
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)338
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)338
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5568
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()19454
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&)352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2352
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&)5580
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()352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()19453
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&)352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2352
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)15614
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)15614
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().2107
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)2
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)146122
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
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&)146122
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()107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)77755
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()72512
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2459
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&)77516
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()459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()72534
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2459
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2151
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2151
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)573
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)573
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1801
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
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&)1801
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3108
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().231
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3108
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().231
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)94
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
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&)94
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)80675
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()147771
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2459
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&)80403
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()459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()147784
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2459
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1803
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
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&)1803
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::start()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().2107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8241
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()95
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8224
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()97
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().29
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)510238
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2321
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&)510030
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()321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2321
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)540241
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()246331
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&)329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2329
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&)540144
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()329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()246332
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&)329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2329
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)18910
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()39907
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2214
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&)18910
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()214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()39907
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2214
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]() const426
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const214
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18702
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const14585
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() const18701
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const14585
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]() const207
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() const17662
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4557
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() const17662
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const4557
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]() const110
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const407896
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const197052
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() const407914
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const197055
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]() const340
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const979691
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const764862
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]() const133
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const979663
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const764891
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]() const863
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const211
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18709
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const303
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() const211
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const18709
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const303
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]() const245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const179470
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const179465
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() const179470
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const179465
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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const453657
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const117754
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const560367
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const428484
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() const453641
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const117754
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const560160
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const428578
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]() const417
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const71950
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const51217
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() const71957
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const51217
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]() const428
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const214
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]() const112
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const710
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() const710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const710
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]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const259
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2655
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const363
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() const259
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2655
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const363
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]() const107
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const37905
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const19453
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]() const108
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const37905
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const19453
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]() const460
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
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]() const107
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]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const108
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const85343
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const72526
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() const85364
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const72535
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]() const459
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const18480
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() const18480
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const147833
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const147781
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const122
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const147859
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const147784
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]() const581
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const97
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const96
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() const97
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const95
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const227722
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const265249
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const227722
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const265252
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const321
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const572552
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const246333
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const572553
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const246333
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]() const330
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const57883
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const39907
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() const39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const57883
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const39907
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]() const214
+
+
+ + + +
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..e2aabe7d55 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html @@ -0,0 +1,446 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-06-08 22:13:12Functions:495104447.4 %
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        7003 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29        7003 :         : m_nh(options.nh),
+      30        7003 :           m_topic_name(options.topic_name),
+      31        7003 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35        7003 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39        7003 :           m_queue_size(options.queue_size),
+      40        7003 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42        7003 :       // initialize the callback for the TimeoutManager
+      43             :       if (options.timeout_callback)
+      44             :         m_timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      45          91 :       else
+      46          91 :         m_timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      47             : 
+      48             :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      49         182 :       {
+      50          91 :         // initialize a new TimeoutManager if not provided by the user
+      51           1 :         if (!m_timeout_manager)
+      52             :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(options.no_message_timeout * 0.5));
+      53          90 : 
+      54             :         // register the timeout callback with the TimeoutManager
+      55             :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, m_timeout_mgr_callback);
+      56          91 :       }
+      57             : 
+      58             :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      59       21009 :       if (m_node_name.empty())
+      60        7003 :         ROS_INFO_STREAM(msg);
+      61           1 :       else
+      62             :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      63        7002 :     }
+      64        7003 :     //}
+      65             : 
+      66             :     virtual ~Impl() = default;
+      67        7003 : 
+      68             :   public:
+      69             :     /* getMsg() method //{ */
+      70             :     virtual typename MessageType::ConstPtr getMsg()
+      71     2178316 :     {
+      72             :       m_new_data = false;
+      73     2178316 :       m_used_data = true;
+      74     2178316 :       return peekMsg();
+      75     2178316 :     }
+      76             :     //}
+      77             : 
+      78             :     /* peekMsg() method //{ */
+      79             :     virtual typename MessageType::ConstPtr peekMsg() const
+      80     2178338 :     {
+      81             :       /* assert(m_got_data); */
+      82             :       /* if (!m_got_data) */
+      83             :       /*   ROS_ERROR("[%s]: No data received yet from topic '%s' (forgot to check hasMsg()?)! Returning empty message.", m_node_name.c_str(), */
+      84             :       /*             topicName().c_str()); */
+      85             :       return m_latest_message;
+      86     2178338 :     }
+      87             :     //}
+      88             : 
+      89             :     /* hasMsg() method //{ */
+      90             :     virtual bool hasMsg() const
+      91     3010406 :     {
+      92             :       return m_got_data;
+      93     3010406 :     }
+      94             :     //}
+      95             : 
+      96             :     /* newMsg() method //{ */
+      97             :     virtual bool newMsg() const
+      98      560160 :     {
+      99             :       return m_new_data;
+     100      560160 :     }
+     101             :     //}
+     102             : 
+     103             :     /* usedMsg() method //{ */
+     104             :     virtual bool usedMsg() const
+     105           0 :     {
+     106             :       return m_used_data;
+     107           0 :     }
+     108             :     //}
+     109             : 
+     110             :     /* waitForNew() method //{ */
+     111             :     virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout)
+     112           0 :     {
+     113             :       // convert the ros type to chrono type
+     114             :       const std::chrono::duration<float> chrono_timeout(timeout.toSec());
+     115           0 :       // lock the mutex guarding the m_new_data flag
+     116             :       std::unique_lock lock(m_new_data_mtx);
+     117           0 :       // if new data is available, return immediately, otherwise attempt to wait for new data using the respective condition variable
+     118             :       if (m_new_data)
+     119           0 :         return getMsg();
+     120           0 :       else if (m_new_data_cv.wait_for(lock, chrono_timeout) == std::cv_status::no_timeout && m_new_data)
+     121           0 :         return getMsg();
+     122           0 :       else
+     123             :         return nullptr;
+     124           0 :     };
+     125             :     //}
+     126             : 
+     127             :     /* lastMsgTime() method //{ */
+     128             :     virtual ros::Time lastMsgTime() const
+     129      721951 :     {
+     130             :       return m_latest_message_time;
+     131      721951 :     };
+     132             :     //}
+     133             : 
+     134             :     /* topicName() method //{ */
+     135             :     virtual std::string topicName() const
+     136        7458 :     {
+     137             :       std::string ret = m_sub.getTopic();
+     138        7458 :       if (ret.empty())
+     139        7458 :         ret = m_nh.resolveName(m_topic_name);
+     140        7094 :       return ret;
+     141        7458 :     }
+     142             :     //}
+     143             : 
+     144             :     /* getNumPublishers() method //{ */
+     145             :     virtual uint32_t getNumPublishers() const
+     146        7007 :     {
+     147             :       return m_sub.getNumPublishers();
+     148        7007 :     };
+     149          95 :     //}
+     150        7007 : 
+     151        7007 :     /* setNoMessageTimeout() method //{ */
+     152             :     virtual void setNoMessageTimeout(const ros::Duration& timeout)
+     153             :     {
+     154             :       if (timeout == mrs_lib::no_timeout)
+     155           5 :       {
+     156             :         // if there is a timeout callback already registered but the user wants to disable it, pause it
+     157           5 :         if (m_timeout_manager != nullptr && m_timeout_id.has_value())
+     158           5 :           m_timeout_manager->pause(m_timeout_id.value());
+     159           5 :         // otherwise, there is no callback, so nothing to do
+     160           5 :       }
+     161             :       else
+     162             :       {
+     163             :         // if there is no callback manager, create it
+     164             :         if (m_timeout_manager == nullptr)
+     165             :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(timeout * 0.5));
+     166             : 
+     167             :         // if there is an existing timeout callback registered, change its timeout
+     168             :         if (m_timeout_id.has_value())
+     169             :           m_timeout_manager->change(m_timeout_id.value(), timeout, m_timeout_mgr_callback);
+     170             :         // otherwise, register it
+     171             :         else
+     172             :           m_timeout_id = m_timeout_manager->registerNew(timeout, m_timeout_mgr_callback);
+     173             :       }
+     174             :     }
+     175             :     //}
+     176             : 
+     177             :     /* start() method //{ */
+     178             :     virtual void start()
+     179             :     {
+     180             :       if (m_timeout_manager && m_timeout_id.has_value())
+     181             :         m_timeout_manager->start(m_timeout_id.value());
+     182             :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     183             :     }
+     184             :     //}
+     185             : 
+     186             :     /* stop() method //{ */
+     187             :     virtual void stop()
+     188             :     {
+     189             :       if (m_timeout_manager && m_timeout_id.has_value())
+     190             :         m_timeout_manager->pause(m_timeout_id.value());
+     191             :       m_sub.shutdown();
+     192             :     }
+     193             :     //}
+     194             : 
+     195           8 :   protected:
+     196             :     ros::NodeHandle m_nh;
+     197           8 :     ros::Subscriber m_sub;
+     198           8 : 
+     199          24 :   protected:
+     200             :     std::string m_topic_name;
+     201           8 :     std::string m_node_name;
+     202           0 : 
+     203             :   protected:
+     204           8 :     bool m_got_data;   // whether any data was received
+     205           8 : 
+     206             :     mutable std::mutex m_new_data_mtx;
+     207             :     mutable std::condition_variable m_new_data_cv;
+     208             :     bool m_new_data;   // whether new data was received since last call to get_data
+     209     6574282 : 
+     210             :     bool m_used_data;  // whether get_data was successfully called at least once
+     211     6574282 : 
+     212     6580521 :   protected:
+     213             :     std::shared_ptr<mrs_lib::TimeoutManager> m_timeout_manager;
+     214             :     std::optional<mrs_lib::TimeoutManager::timeout_id_t> m_timeout_id;
+     215     6577218 :     mrs_lib::TimeoutManager::callback_t m_timeout_mgr_callback;
+     216     6571472 : 
+     217     6571472 :   protected:
+     218     6580081 :     ros::Time m_latest_message_time;
+     219             :     typename MessageType::ConstPtr m_latest_message;
+     220             :     message_callback_t m_message_callback;
+     221             : 
+     222           0 :   private:
+     223             :     uint32_t m_queue_size;
+     224             :     ros::TransportHints m_transport_hints;
+     225           0 : 
+     226           0 :   protected:
+     227           0 :     /* default_timeout_callback() method //{ */
+     228           0 :     void default_timeout_callback(const std::string& topic_name, const ros::Time& last_msg)
+     229             :     {
+     230             :       const ros::Duration since_msg = (ros::Time::now() - last_msg);
+     231             :       const auto n_pubs = m_sub.getNumPublishers();
+     232           0 :       const std::string txt = "Did not receive any message from topic '" + topic_name + "' for " + std::to_string(since_msg.toSec()) + "s ("
+     233           0 :                               + std::to_string(n_pubs) + " publishers on this topic)";
+     234           0 :       if (m_node_name.empty())
+     235             :         ROS_WARN_STREAM(txt);
+     236             :       else
+     237             :         ROS_WARN_STREAM("[" << m_node_name << "]: " << txt);
+     238             :     }
+     239             :     //}
+     240             : 
+     241             :     /* process_new_message() method //{ */
+     242             :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     243             :     {
+     244             :       m_latest_message_time = ros::Time::now();
+     245             :       m_latest_message = msg;
+     246             :       // If the message callback is registered, the new data will immediately be processed,
+     247             :       // so reset the flag. Otherwise, set the flag.
+     248             :       m_new_data = !m_message_callback;
+     249             :       m_got_data = true;
+     250             :       m_new_data_cv.notify_one();
+     251             :     }
+     252             :     //}
+     253        7003 : 
+     254        7003 :     /* data_callback() method //{ */
+     255             :     virtual void data_callback(const typename MessageType::ConstPtr& msg)
+     256        7003 :     {
+     257             :       {
+     258             :         std::lock_guard lck(m_new_data_mtx);
+     259     3010359 :         if (m_timeout_manager && m_timeout_id.has_value())
+     260             :           m_timeout_manager->reset(m_timeout_id.value());
+     261     6020683 :         process_new_message(msg);
+     262     6020810 :       }
+     263             : 
+     264      560367 :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     265             :       if (m_message_callback)
+     266     1119654 :         m_message_callback(msg);
+     267     1118761 :     }
+     268             :     //}
+     269           0 :   };
+     270             :   //}
+     271           0 : 
+     272           0 :   /* SubscribeHandler_threadsafe class //{ */
+     273             :   template <typename MessageType>
+     274     2178237 :   class SubscribeHandler<MessageType>::ImplThreadsafe : public SubscribeHandler<MessageType>::Impl
+     275             :   {
+     276     4356423 :   private:
+     277     4356153 :     using impl_class_t = SubscribeHandler<MessageType>::Impl;
+     278             : 
+     279     2178201 :   public:
+     280             :     using timeout_callback_t = typename impl_class_t::timeout_callback_t;
+     281     4356133 :     using message_callback_t = typename impl_class_t::message_callback_t;
+     282     4355901 : 
+     283             :     friend class SubscribeHandler<MessageType>;
+     284      721967 : 
+     285             :   public:
+     286     1443775 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     287     1443581 :         : impl_class_t::Impl(options, message_callback)
+     288             :     {
+     289         364 :     }
+     290             : 
+     291         728 :   public:
+     292         728 :     virtual bool hasMsg() const override
+     293             :     {
+     294        7007 :       std::lock_guard lck(m_mtx);
+     295             :       return impl_class_t::hasMsg();
+     296       14014 :     }
+     297       14014 :     virtual bool newMsg() const override
+     298             :     {
+     299           5 :       std::lock_guard lck(m_mtx);
+     300             :       return impl_class_t::newMsg();
+     301          10 :     }
+     302          10 :     virtual bool usedMsg() const override
+     303             :     {
+     304             :       std::lock_guard lck(m_mtx);
+     305       14006 :       return impl_class_t::usedMsg();
+     306             :     }
+     307             :     virtual typename MessageType::ConstPtr getMsg() override
+     308     6578169 :     {
+     309             :       std::lock_guard lck(m_mtx);
+     310             :       return impl_class_t::getMsg();
+     311    13154885 :     }
+     312     6556499 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     313      164173 :     {
+     314     6530427 :       std::lock_guard lck(m_mtx);
+     315             :       return impl_class_t::peekMsg();
+     316             :     }
+     317             :     virtual ros::Time lastMsgTime() const override
+     318     6558231 :     {
+     319     3600923 :       std::lock_guard lck(m_mtx);
+     320     6551264 :       return impl_class_t::lastMsgTime();
+     321             :     };
+     322             :     virtual std::string topicName() const override
+     323             :     {
+     324             :       std::lock_guard lck(m_mtx);
+     325             :       return impl_class_t::topicName();
+     326             :     };
+     327             :     virtual void start() override
+     328             :     {
+     329             :       std::lock_guard lck(m_mtx);
+     330             :       return impl_class_t::start();
+     331             :     }
+     332             :     virtual void stop() override
+     333             :     {
+     334             :       std::lock_guard lck(m_mtx);
+     335             :       return impl_class_t::stop();
+     336             :     }
+     337             : 
+     338             :     virtual ~ImplThreadsafe() override = default;
+     339             : 
+     340             :   protected:
+     341             :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     342             :     {
+     343             :       {
+     344             :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     345             :         if (this->m_timeout_manager && this->m_timeout_id.has_value())
+     346             :           this->m_timeout_manager->reset(this->m_timeout_id.value());
+     347             :         impl_class_t::process_new_message(msg);
+     348             :       }
+     349             : 
+     350             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     351             :       if (this->m_message_callback)
+     352             :         impl_class_t::m_message_callback(msg);
+     353             :     }
+     354             : 
+     355             :   private:
+     356             :     mutable std::recursive_mutex m_mtx;
+     357             :   };
+     358             :   //}
+     359             : 
+     360             : }  // namespace mrs_lib
+     361             : 
+     362             : #endif  // SUBSCRIBE_HANDLER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html new file mode 100644 index 0000000000..c387463802 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html @@ -0,0 +1,111 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..f0f4fe08ac542279ba7bd32e565761c46af715d0 GIT binary patch literal 1636 zcmV-q2AlbbP);K!HeN2QT|Cl6-$pli+AhpdzZ{q8b zmBiY}&5U`QtV^@*0WE>h&qy_i$_U|+OBLlmi8qBEO;XYfKd@~tzxG#IF1=CS`@H%9 zx6J`$>#hLYUe#hgzydkK6>z{hn@HwSi9=Xr7)e`T$3zwDnVfAvx5EC&;G9iAz=4A+ ziZM)Fj+#A(jpKn5^WZ&d8Uw_40>wcrP^}T;ornSAqzD&jz0lx{zIN`#FQ?ENSVysvzl*roJWJPmIPO*E5doKy?QPEXHl*O3`pw5x zpNzQCc${cGxA%549$k{c`#8KNK=CddCFF{=2nh9FG}|OUidYY=M_Ho^W_Wfb7~YG9 z$&jjSmQ%#&BLMXKN&wu)p|gF(*NO#2VZX*48r*-mZE#d&^s`wIGVS28h#%mR#q&Eq z$!lT>9mMs7A}}nh3?FW*sQ>tV7s~fcLidr%J+qIq==N7c`2Xe^nn#`hxIItS`+dG8 z>jQjW*L7TejXt8GI3!enYV!!g4eZQf*O>rR{8dzUd|(E_A|s z5)Z};X`bYNTTMP<-@KE6cgT(}ez}%)GKNdUfG`D$m66Q=lok)Yi-AD5omK_!0u?uQ$=HpZO2$UAvfUBI`1Yi>7i$yG1#Wtk-}5l33+1;YPh3S& zipdwV!X8!j6N+~(uq^dD$DDO_i>m|HkZ<9oM#k?LT+(P3gU>|xU4wUiaaYEt4Zh85 zqnw)I zX3`LpAxJvK@n4TLOA!vQ7BrKJ!v#=2y~6u7orUChug{9N7xGN9ooAk2<*#9;bsTBl zgBG3;9mkQULnod|jSZUTJ=ro7t)-!c!Xk|#!mVXJTK(<%vFxwGLrApZsGr9%Aey3_ zDds7WGXfejF+i~I3e7??z0F5L{4oRL`!==B~k;wp*5(2dB3Pg^ohy>Y2?&wE_osq0##bO z7s)(7mXVlclt?R6cY*RG^=~SZv%@_j6bTlZ0nRSd8SpS8{xuBJNC+#ce%Qsne3UR39~L%(`V*q z-yUiz?kdwqD#Ofl9ipdrSD7bRD?}_!a7cO+R|*IU2NflHs&orK$3HJpXssUtluN#j z@yV8{?msAe9W6V={p^pREY#FO0hP9ULyej0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-06-08 22:13:12Functions: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..d42c7da2ab --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-06-08 22:13:12Functions: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..6d511759d4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html @@ -0,0 +1,180 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-06-08 22:13:12Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::doTransform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
geometry_msgs::Quaternion_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Point_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
cv::Point3_<double> mrs_lib::Transformer::copyChangeFrame<cv::Point3_<double> >(cv::Point3_<double> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
Eigen::Quaternion<double, 0> mrs_lib::Transformer::copyChangeFrame<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<cv::Point3_<double> > mrs_lib::Transformer::doTransform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transformImpl<cv::Point3_<double> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, cv::Point3_<double> const&)1
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transformImpl<Eigen::Quaternion<double, 0> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, Eigen::Quaternion<double, 0> const&)2
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)2
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)4
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)5
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)6
std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > mrs_lib::Transformer::transform<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Point_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)18
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Vector3_<std::allocator<void> > const&)18
geometry_msgs::Vector3Stamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)94
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&)737
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&)21425
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&)21425
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)21425
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&)25536
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)25536
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&)98848
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&)105817
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&)193415
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)193415
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&)232082
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&)257376
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&)257603
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&)257618
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)283103
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&)628156
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&)628726
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&)673921
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&)813546
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&)813590
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)813716
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&)814297
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&)814390
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)822079
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&)867334
+
+
+ + + +
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..9821e0cb14 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html @@ -0,0 +1,272 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-06-08 22:13:12Functions:424887.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)673921
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&)232082
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&)814297
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&)867334
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&)257618
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&)814390
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&)193415
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&)25536
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&)94
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&)628726
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&)628156
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&)257376
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&)257603
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&)813546
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&)813590
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&)21425
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&)21425
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)822079
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)283103
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)813716
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)21425
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)193415
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)25536
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)94
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&)105817
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&)737
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&)98848
+
+
+ + + +
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..59b18f8c77 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-06-08 22:13:12Functions:424887.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef TRANSFORMER_HPP
+       2             : #define TRANSFORMER_HPP
+       3             : 
+       4             : // clang: MatousFormat
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             : 
+       9             :   // | --------------------- helper methods --------------------- |
+      10             : 
+      11             :   /* getHeader() overloads for different message types (pointers, pointclouds etc) //{ */
+      12             : 
+      13             :   template <typename msg_t>
+      14     1940323 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16     1940323 :     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      219045 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34      219045 :     msg.header = header;
+      35      219045 :   }
+      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      219045 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50      219045 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53      438090 :       std_msgs::Header new_header = getHeader(what);
+      54      219045 :       new_header.frame_id = frame_id;
+      55      219045 :       setHeader(ret, new_header);
+      56             :     }
+      57      219045 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65     1939385 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67     3878749 :     const std::string from_frame = frame_from(tf);
+      68     3878742 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70     1939375 :     if (from_frame == to_frame)
+      71      219045 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73     3440671 :     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      906017 :       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      906015 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90        4242 :         const std::optional<T> tmp = doTransform(what, tf);
+      91        2122 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93        2122 :         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      814324 :       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     1718214 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115     1720341 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119     3440592 :       T result;
+     120     1720322 :       tf2::doTransform(what, result, tf);
+     121     1720326 :       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     1720774 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140     3442732 :     const std_msgs::Header orig_header = getHeader(what);
+     141     3441943 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145     1721074 :   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     3443029 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149     1721979 :     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     3443928 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156     3443910 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157     3443910 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160     3443933 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161     1721975 :     if (!tf_opt.has_value())
+     162       14547 :       return std::nullopt;
+     163     1707417 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166     3414839 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167     1707426 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175      205420 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177      410840 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179      205420 :     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      410840 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186      410840 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187      410840 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189      205420 :     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..6b815308bb --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-06-08 22:13:12Functions: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..04acb07185 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-06-08 22:13:12Functions: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..ea03bbf5cb --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-06-08 22:13:12Functions:81266.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

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

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

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::varstepLKF<3, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 3, 3, 0, 3, 3> (double)> const&, std::function<Eigen::Matrix<double, 3, 1, 0, 3, 1> (double)> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)0
mrs_lib::varstepLKF<6, 2, 2>::varstepLKF(std::function<Eigen::Matrix<double, 6, 6, 0, 6, 6> (double)> const&, std::function<Eigen::Matrix<double, 6, 2, 0, 6, 2> (double)> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)0
mrs_lib::LKF<2, 1, 1>::LKF(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)0
mrs_lib::LKF<3, 1, 1>::LKF()0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 4, 1, 0, 4, 1> >::type mrs_lib::LKF<4, 1, 2>::state_predict<1>(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)0
mrs_lib::LKF<4, 1, 2>::covariance_predict(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double)0
mrs_lib::LKF<4, 1, 2>::correction_optimized(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)0
mrs_lib::LKF<4, 1, 2>::LKF(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::LKF()0
mrs_lib::LKF<6, 2, 2>::LKF()0
mrs_lib::varstepLKF<3, 1, 1>::predict(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double) const0
mrs_lib::varstepLKF<6, 2, 2>::predict(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double) const0
mrs_lib::LKF<2, 1, 1>::inverse_exception::what() const0
mrs_lib::LKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const0
mrs_lib::LKF<3, 1, 1>::inverse_exception::what() const0
std::enable_if<(4)>=(0), mrs_lib::KalmanFilter<4, 1, 2>::statecov_t>::type mrs_lib::LKF<4, 1, 2>::correction_impl<4>(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::computeKalmanGain(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::inverse_exception::what() const0
mrs_lib::LKF<4, 1, 2>::correct(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const0
mrs_lib::LKF<4, 1, 2>::predict(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double) const0
mrs_lib::LKF<6, 2, 2>::inverse_exception::what() const0
mrs_lib::varstepLKF<2, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 2, 2, 0, 2, 2> (double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (double)> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)3
mrs_lib::LKF<2, 1, 1>::LKF()3
mrs_lib::LKF<6, 2, 2>::LKF(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)112
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&)198
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5830
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&) const5830
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&) const5830
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&) const5830
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&)17232
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)17232
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) const17232
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) const185304
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)185311
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&)185322
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)212110
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&) const212267
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&) const212354
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&) const212367
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) const323558
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&)323644
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)323743
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)492911
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&) const493359
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&) const494330
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&) const494355
+
+
+ + + +
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..aee9d448e7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-06-08 22:13:12Functions:254753.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::varstepLKF<2, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 2, 2, 0, 2, 2> (double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (double)> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)3
mrs_lib::varstepLKF<3, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 3, 3, 0, 3, 3> (double)> const&, std::function<Eigen::Matrix<double, 3, 1, 0, 3, 1> (double)> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)0
mrs_lib::varstepLKF<6, 2, 2>::varstepLKF(std::function<Eigen::Matrix<double, 6, 6, 0, 6, 6> (double)> const&, std::function<Eigen::Matrix<double, 6, 2, 0, 6, 2> (double)> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 2, 1, 0, 2, 1> >::type mrs_lib::LKF<2, 1, 1>::state_predict<1>(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)17232
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)17232
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5830
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&)323644
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)323743
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)492911
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&)198
mrs_lib::LKF<3, 1, 1>::LKF()0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 4, 1, 0, 4, 1> >::type mrs_lib::LKF<4, 1, 2>::state_predict<1>(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)0
mrs_lib::LKF<4, 1, 2>::covariance_predict(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double)0
mrs_lib::LKF<4, 1, 2>::correction_optimized(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)0
mrs_lib::LKF<4, 1, 2>::LKF(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::LKF()0
std::enable_if<(2)!=(0), Eigen::Matrix<double, 6, 1, 0, 6, 1> >::type mrs_lib::LKF<6, 2, 2>::state_predict<2>(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)185322
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)185311
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)212110
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&)112
mrs_lib::LKF<6, 2, 2>::LKF()0
mrs_lib::varstepLKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const17232
mrs_lib::varstepLKF<3, 1, 1>::predict(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, double) const0
mrs_lib::varstepLKF<6, 2, 2>::predict(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, double) const0
std::enable_if<(2)>=(0), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::LKF<2, 1, 1>::correction_impl<2>(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&) const5830
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&) const5830
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&) const5830
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&) const494330
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&) const493359
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&) const494355
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) const323558
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&) const212354
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&) const212367
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&) const212267
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) const185304
+
+
+ + + +
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..401da5e2d5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.gcov.html @@ -0,0 +1,460 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-06-08 22:13:12Functions:254753.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file LKF
+       3             :      \brief Defines LKF - a class, implementing the Linear Kalman Filter \cite LKF, as well as a few specialized variants.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : #ifndef LKFSYSTEMMODELS_H
+       7             : #define LKFSYSTEMMODELS_H
+       8             : 
+       9             : #include <mrs_lib/kalman_filter.h>
+      10             : #include <iostream>
+      11             : 
+      12             : namespace mrs_lib
+      13             : {
+      14             : 
+      15             :   /* class LKF //{ */
+      16             :   /**
+      17             :   * \brief Implementation of the Linear Kalman filter \cite LKF.
+      18             :   *
+      19             :   * The Linear Kalman filter (abbreviated LKF, \cite LKF) may be used for state filtration or estimation of linear
+      20             :   * stochastic discrete systems. It assumes that noise variables are sampled from multivariate gaussian distributions
+      21             :   * and takes into account apriori known parameters of these distributions (namely zero means and covariance matrices,
+      22             :   * which have to be specified by the user and are tunable parameters).
+      23             :   *
+      24             :   * The LKF C++ class itself is templated. This has its advantages and disadvantages. Main disadvantage is that it
+      25             :   * may be harder to use if you're not familiar with C++ templates, which, admittedly, can get somewhat messy,
+      26             :   * espetially during compilation. Another disadvantage is that if used unwisely, the compilation times can get
+      27             :   * much higher when using templates. The main advantage is compile-time checking (if it compiles, then it has
+      28             :   * a lower chance of crashing at runtime) and enabling more effective optimalizations during compilation. Also in case
+      29             :   * of Eigen, the code is arguably more readable when you use aliases to the specific Matrix instances instead of
+      30             :   * having Eigen::MatrixXd and Eigen::VectorXd everywhere.
+      31             :   *
+      32             :   * \tparam n_states         number of states of the system (length of the \f$ \mathbf{x} \f$ vector).
+      33             :   * \tparam n_inputs         number of inputs of the system (length of the \f$ \mathbf{u} \f$ vector).
+      34             :   * \tparam n_measurements   number of measurements of the system (length of the \f$ \mathbf{z} \f$ vector).
+      35             :   *
+      36             :   */
+      37             :   template <int n_states, int n_inputs, int n_measurements>
+      38             :   class LKF : public KalmanFilter<n_states, n_inputs, n_measurements>
+      39             :   {
+      40             :   public:
+      41             :     /* LKF definitions (typedefs, constants etc) //{ */
+      42             :     static constexpr int n = n_states;            /*!< \brief Length of the state vector of the system. */
+      43             :     static constexpr int m = n_inputs;            /*!< \brief Length of the input vector of the system. */
+      44             :     static constexpr int p = n_measurements;      /*!< \brief Length of the measurement vector of the system. */
+      45             :     using Base_class = KalmanFilter<n, m, p>; /*!< \brief Base class of this class. */
+      46             : 
+      47             :     using x_t = typename Base_class::x_t;                /*!< \brief State vector type \f$n \times 1\f$ */
+      48             :     using u_t = typename Base_class::u_t;                /*!< \brief Input vector type \f$m \times 1\f$ */
+      49             :     using z_t = typename Base_class::z_t;                /*!< \brief Measurement vector type \f$p \times 1\f$ */
+      50             :     using P_t = typename Base_class::P_t;                /*!< \brief State uncertainty covariance matrix type \f$n \times n\f$ */
+      51             :     using R_t = typename Base_class::R_t;                /*!< \brief Measurement noise covariance matrix type \f$p \times p\f$ */
+      52             :     using Q_t = typename Base_class::Q_t;                /*!< \brief Process noise covariance matrix type \f$n \times n\f$ */
+      53             :     using statecov_t = typename Base_class::statecov_t;  /*!< \brief Helper struct for passing around the state and its covariance in one variable */
+      54             : 
+      55             :     typedef Eigen::Matrix<double, n, n> A_t;  /*!< \brief System transition matrix type \f$n \times n\f$ */
+      56             :     typedef Eigen::Matrix<double, n, m> B_t;  /*!< \brief Input to state mapping matrix type \f$n \times m\f$ */
+      57             :     typedef Eigen::Matrix<double, p, n> H_t;  /*!< \brief State to measurement mapping matrix type \f$p \times n\f$ */
+      58             :     typedef Eigen::Matrix<double, n, p> K_t;  /*!< \brief Kalman gain matrix type \f$n \times p\f$ */
+      59             : 
+      60             :   /*!
+      61             :     * \brief This exception is thrown when taking the inverse of a matrix fails.
+      62             :     *
+      63             :     * You should catch this exception in your code and act accordingly if it appears
+      64             :     * (e.g. reset the state and covariance or modify the measurement/process noise parameters).
+      65             :     */
+      66             :     struct inverse_exception : public std::exception
+      67             :     {
+      68             :     /*!
+      69             :       * \brief Returns the error message, describing what caused the exception.
+      70             :       *
+      71             :       * \return  The error message, describing what caused the exception.
+      72             :       */
+      73           0 :       const char* what() const throw()
+      74             :       {
+      75           0 :         return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
+      76             :       }
+      77             :     };
+      78             :     //}
+      79             : 
+      80             :   public:
+      81             :   /*!
+      82             :     * \brief Convenience default constructor.
+      83             :     *
+      84             :     * This constructor should not be used if applicable. If used, the main constructor has to be called afterwards,
+      85             :     * before using this class, otherwise the LKF object is invalid (not initialized).
+      86             :     */
+      87           3 :     LKF(){};
+      88             : 
+      89             :   /*!
+      90             :     * \brief The main constructor.
+      91             :     *
+      92             :     * \param A             The state transition matrix.
+      93             :     * \param B             The input to state mapping matrix.
+      94             :     * \param H             The state to measurement mapping matrix.
+      95             :     */
+      96         310 :     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      712452 :     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      712452 :       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      508862 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140      508862 :       statecov_t ret;
+     141      507804 :       ret.x = state_predict(A, sc.x, B, u);
+     142      508205 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143      507393 :       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      526286 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156      526286 :       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      526198 :     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      526198 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177      710851 :     static R_t invert_W(R_t W)
+     178             :     {
+     179      710851 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180      712229 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183       47805 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184       47809 :         W += 1e-9 * ident;
+     185       47837 :         qr.compute(W);
+     186       47847 :         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      709431 :       const R_t W_inv = qr.inverse();
+     193     1421302 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198      711556 :     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      711556 :       const R_t W = H * sc.P * H.transpose() + R;
+     202      711870 :       const R_t W_inv = invert_W(W);
+     203      709115 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204     1423604 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210      712514 :     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      712514 :       statecov_t ret;
+     214      711805 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215      712094 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216      710865 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217     1421818 :       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       17232 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       17232 :       statecov_t ret;
+     319       17232 :       A_t A = m_generateA(dt);
+     320       17232 :       B_t B = m_generateB(dt);
+     321       17232 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       17232 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       34464 :       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..794b8a5821 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-06-08 22:13:12Functions:154434.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::getHeading(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::Reference_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::TwistWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<geometry_msgs::Twist_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::Reference_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getPosition(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(geometry_msgs::TwistWithCovariance_<std::allocator<void> > const&)0
mrs_lib::getVelocity(geometry_msgs::Twist_<std::allocator<void> > const&)0
mrs_lib::getVelocity(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getVelocity(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getVelocity(nav_msgs::Odometry_<std::allocator<void> > const&)0
mrs_lib::getXYZ(geometry_msgs::Vector3_<std::allocator<void> > const&)0
mrs_lib::getXYZ(boost::shared_ptr<geometry_msgs::Point_<std::allocator<void> > const> const&)0
mrs_lib::getXYZ(boost::shared_ptr<geometry_msgs::Vector3_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)0
mrs_lib::getYaw(geometry_msgs::Pose_<std::allocator<void> > const&)0
mrs_lib::getYaw(boost::shared_ptr<geometry_msgs::PoseWithCovariance_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(boost::shared_ptr<geometry_msgs::Pose_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getYaw(nav_msgs::Odometry_<std::allocator<void> > const&)0
mrs_lib::getPose(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)359
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)359
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)359
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)359
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)359
mrs_lib::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)1798
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)242661
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)242662
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)242664
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)242664
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)242665
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)242665
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)242665
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)242665
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)243383
+
+
+ + + +
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..2663fe584c --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.func.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-06-08 22:13:12Functions:154434.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)242662
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)242664
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&)242661
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)359
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)359
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)242664
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)242665
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)242665
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&)242665
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)359
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)359
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)359
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)242665
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&)243383
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&)1798
+
+
+ + + +
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..c900ba06ca --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html @@ -0,0 +1,775 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-06-08 22:13:12Functions:154434.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: TomasFormat
+       2             : /**  \file
+       3             :  *   \brief utility functions for getting stuff from ROS msgs
+       4             :  *   \author Tomas Baca - tomas.baca@fel.cvut.cz
+       5             :  */
+       6             : #ifndef MRS_LIB_MSG_EXTRACTOR_H
+       7             : #define MRS_LIB_MSG_EXTRACTOR_H
+       8             : 
+       9             : #include <mrs_msgs/TrackerCommand.h>
+      10             : #include <mrs_msgs/Reference.h>
+      11             : #include <mrs_msgs/ReferenceStamped.h>
+      12             : 
+      13             : #include <nav_msgs/Odometry.h>
+      14             : 
+      15             : #include <mrs_lib/attitude_converter.h>
+      16             : 
+      17             : namespace mrs_lib
+      18             : {
+      19             : 
+      20             : /* geometry_msgs::Point //{ */
+      21             : 
+      22             : /**
+      23             :  * @brief get XYZ from geometry_msgs::Point
+      24             :  *
+      25             :  * @param data point
+      26             :  *
+      27             :  * @return x, y, z
+      28             :  */
+      29      243383 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
+      30             : 
+      31      243383 :   double x = data.x;
+      32      243383 :   double y = data.y;
+      33      243383 :   double z = data.z;
+      34             : 
+      35      486763 :   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      242665 : std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
+      96             : 
+      97      242665 :   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      242664 : double getHeading(const geometry_msgs::Pose& data) {
+     124             : 
+     125      242664 :   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      242665 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
+     184             : 
+     185      242665 :   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      242662 : double getHeading(const geometry_msgs::PoseWithCovariance& data) {
+     212             : 
+     213      242662 :   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      242665 : std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
+     336             : 
+     337      242665 :   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      242665 : std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
+     348             : 
+     349      242665 :   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      242664 : double getHeading(const nav_msgs::Odometry& data) {
+     392             : 
+     393      242664 :   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      242661 : double getHeading(const nav_msgs::OdometryConstPtr& data) {
+     404             : 
+     405      242661 :   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        1798 : geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
+     448             : 
+     449        1798 :   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         359 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
+     480             : 
+     481         359 :   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         359 : std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
+     579             : 
+     580         359 :   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         359 : double getHeading(const mrs_msgs::Reference& data) {
+     607             : 
+     608         359 :   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         359 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
+     639             : 
+     640         359 :   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         359 : double getHeading(const mrs_msgs::ReferenceStamped& data) {
+     667             : 
+     668         359 :   return getHeading(data.reference);
+     669             : }
+     670             : 
+     671             : /**
+     672             :  * @brief get heading from mrs_msgs::ReferenceStampedConstPtr
+     673             :  *
+     674             :  * @param data referencestamped (ContrPtr)
+     675             :  *
+     676             :  * @return heading
+     677             :  */
+     678           0 : double getHeading(const mrs_msgs::ReferenceStampedConstPtr& data) {
+     679             : 
+     680           0 :   return getHeading(*data);
+     681             : }
+     682             : 
+     683             : //}
+     684             : 
+     685             : //}
+     686             : 
+     687             : }  // namespace mrs_lib
+     688             : 
+     689             : //}
+     690             : 
+     691             : #endif  // MRS_LIB_MSG_EXTRACTOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html new file mode 100644 index 0000000000..41496f11e9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html @@ -0,0 +1,193 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
std::tuple<double, double, double, double> mrs_lib::get_mutexed<double, double, double, double>(std::mutex&, double&, double&, double&, double&)0
mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const>(std::mutex&, mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)0
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&)0
Eigen::Matrix<double, 2, 2, 0, 2, 2> mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::SpeedTrackerCommand_<std::allocator<void> >&)0
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 1, 0, 2, 1> >(std::mutex&, Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(std::mutex&, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(std::mutex&, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 1, 0, 6, 1> >(std::mutex&, Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Matrix<double, 6, 1, 0, 6, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 6, 0, 6, 6> >(std::mutex&, Eigen::Matrix<double, 6, 6, 0, 6, 6>, Eigen::Matrix<double, 6, 6, 0, 6, 6>&)0
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)1
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)39
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)107
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> >&)158
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)214
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)215
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)435
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)736
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)1107
auto mrs_lib::set_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)2446
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&)4659
int mrs_lib::get_mutexed<int>(std::mutex&, int&)9231
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&)12541
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)15588
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&)15864
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)17020
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)18762
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&)19332
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&)23266
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)27872
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&)33398
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> >&)36943
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)59342
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&)79929
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)79929
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&)85083
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)86608
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)86608
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> >&)88298
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)109824
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> >&)153229
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)155029
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)162168
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>&)185277
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&)196910
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&)197949
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&)205279
auto mrs_lib::set_mutexed<std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >(std::mutex&, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&)226421
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>&)227522
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> >&)246284
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>&)256877
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>&)271290
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)291095
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&)307441
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>&)322590
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&)339313
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&)340490
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&)368195
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> >&)395894
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&)397636
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&)397660
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> >&)549113
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&)599168
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> > >&)709117
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)717813
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)810572
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&)816842
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&)818021
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)968777
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)1009200
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)1041281
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> >&)1054874
double mrs_lib::get_mutexed<double>(std::mutex&, double&)1120707
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)1154551
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&)2535551
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&)2647136
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&)2750325
+
+
+ + + +
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..060780dfe8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func.html @@ -0,0 +1,400 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-06-08 22:13:12Functions:678083.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::tuple<Eigen::Matrix<double, -1, 1, 0, -1, 1>, double> mrs_lib::get_mutexed<Eigen::Matrix<double, -1, 1, 0, -1, 1>, double>(std::mutex&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, double&)79929
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>&)256877
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&)339313
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>&)227522
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&)12541
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)86608
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)17020
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)27872
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&)23266
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)15588
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&)2647136
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&)196910
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&)599168
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&)205279
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&)307441
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&)197949
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&)368195
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&)2535551
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&)2750325
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&)19332
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&)340490
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)717813
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)810572
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)86608
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&)85083
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&)33398
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)155029
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)59342
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)39
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)968777
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>&)322590
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>&)185277
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>&)271290
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)435
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)1107
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)736
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)291095
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> >&)79929
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)1
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)162168
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)1041281
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&)818021
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&)397660
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> >&)88298
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> >&)246284
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> > >&)709117
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)18762
double mrs_lib::get_mutexed<double>(std::mutex&, double&)1120707
int mrs_lib::get_mutexed<int>(std::mutex&, int&)9231
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)1009200
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)109824
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&)15864
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&)4659
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)214
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)215
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)107
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> >&)153229
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> >&)2446
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> >&)549113
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> >&)158
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> >&)1054874
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> >&)395894
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&)816842
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&)397636
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> >&)36943
auto mrs_lib::set_mutexed<std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > >(std::mutex&, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >, std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&)226421
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)1154551
+
+
+ + + +
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..0bd543e687 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.gcov.html @@ -0,0 +1,260 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-06-08 22:13:12Functions:678083.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines helper routines for getting and setting variables under mutex locks
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef MUTEX_H
+       6             : #define MUTEX_H
+       7             : 
+       8             : #include <iostream>
+       9             : #include <mutex>
+      10             : #include <tuple>
+      11             : 
+      12             : namespace mrs_lib
+      13             : {
+      14             : 
+      15             : /**
+      16             :  * @brief thread-safe getter and setter for values of variables (args)
+      17             :  *
+      18             :  * @tparam GetArgs types of the variables to get
+      19             :  * @tparam SetArgs types of the variables to set
+      20             :  * @param mut mutex which protects the variables
+      21             :  * @param get tuple of variable references to obtain the values from
+      22             :  * @param to_set tuple of variable references to set the new values from \p from_set
+      23             :  * @param from_set tuple of the new values to be set to \p to_set
+      24             :  *
+      25             :  * @return tuple of the values from \p get
+      26             :  */
+      27             : template <class... GetArgs, class... SetArgs>
+      28             : std::tuple<GetArgs...> get_set_mutexed(std::mutex& mut, std::tuple<GetArgs&...> get, std::tuple<SetArgs...> from_set, std::tuple<SetArgs&...> to_set) {
+      29             : 
+      30             :   std::scoped_lock lock(mut);
+      31             : 
+      32             :   std::tuple<GetArgs...> result = get;
+      33             :   to_set = from_set;
+      34             : 
+      35             :   return result;
+      36             : }
+      37             : 
+      38             : /**
+      39             :  * @brief thread-safe getter for values of variables (args)
+      40             :  *
+      41             :  * @tparam Args types of the variables
+      42             :  * @param mut mutex which protects the variables
+      43             :  * @param args variables to obtain the values from
+      44             :  *
+      45             :  * @return std::tuple of the values
+      46             :  */
+      47             : template <class... Args>
+      48     1086536 : std::tuple<Args...> get_mutexed(std::mutex& mut, Args&... args) {
+      49             : 
+      50     2173072 :   std::scoped_lock lock(mut);
+      51             : 
+      52     1086536 :   std::tuple result = std::tuple(args...);
+      53             : 
+      54     2173072 :   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    19857626 : T get_mutexed(std::mutex& mut, T& arg) {
+      69             : 
+      70    32646397 :   std::scoped_lock lock(mut);
+      71             : 
+      72    39712043 :   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     4918990 : auto set_mutexed(std::mutex& mut, const T what, T& where) {
+     117             : 
+     118     8655519 :   std::scoped_lock lock(mut);
+     119             : 
+     120     4910653 :   where = what;
+     121             : 
+     122     9832753 :   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..866df9e4a4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html @@ -0,0 +1,392 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-06-08 22:13:12Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixStatic<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>&)1
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
std::pair<Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
bool mrs_lib::ParamLoader::loadMatrixKnown<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)2
bool mrs_lib::ParamLoader::loadMatrixKnown<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)2
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)3
mrs_lib::ParamLoader::resetLoadedSuccessfully()3
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
mrs_lib::ParamLoader::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)5
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5
std::pair<Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
std::pair<XmlRpc::XmlRpcValue, bool> mrs_lib::ParamLoader::load<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)8
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)10
mrs_lib::ParamLoader::resetUniques()20
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)39
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)86
bool mrs_lib::ParamLoader::loadMatrixDynamic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)88
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > > const&)107
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > > const&, int, int)107
std::pair<Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)107
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixDynamic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)197
bool mrs_lib::ParamLoader::loadMatrixKnown<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)428
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)428
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&)535
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)738
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&)742
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)856
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&)1177
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)1177
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> >&)1177
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1811
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2134
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2864
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&)3750
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)3751
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> > > >&)3751
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)4167
mrs_lib::ParamLoader::loadedSuccessfully()4670
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)5418
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)5418
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)5418
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)6329
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)11885
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)12566
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> >&)17503
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&)18078
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)18078
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)18895
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)18895
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)42940
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)43796
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)43796
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)91872
+
+
+ + + +
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..4d8fee5bc6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func.html @@ -0,0 +1,392 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-06-08 22:13:12Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::ParamLoader::loadParam2<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)535
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
void mrs_lib::ParamLoader::printValue<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)3750
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&)18078
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)18895
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&)742
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&)1177
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)43796
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)8
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)5418
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)11885
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)738
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)10
mrs_lib::ParamLoader::resetUniques()20
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
bool mrs_lib::ParamLoader::loadMatrixKnown<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)428
bool mrs_lib::ParamLoader::loadMatrixKnown<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)2
bool mrs_lib::ParamLoader::loadMatrixKnown<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixStatic<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>&)1
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 3, 3, 0, 3, 3> > > const&)107
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)3
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)2
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)88
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<double>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > > const&, int, int)107
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)86
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
mrs_lib::ParamLoader::loadedSuccessfully()4670
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2864
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&)91872
mrs_lib::ParamLoader::resetLoadedSuccessfully()3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)428
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
std::pair<Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
std::pair<Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)107
std::pair<Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
std::pair<Eigen::Matrix<double, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixDynamic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)197
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)18078
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)3751
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)1177
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)18895
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)43796
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)5418
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)17503
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)39
bool mrs_lib::ParamLoader::loadParam<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&)3751
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> >&)1177
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)12566
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)6329
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)42940
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)856
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)5418
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2134
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1811
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)4167
+
+
+ + + +
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..287c13d698 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html @@ -0,0 +1,1452 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-06-08 22:13:12Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()0
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler()1
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler_impl()1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler()4
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()31
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()62
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()108
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()112
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()215
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()220
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()220
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()220
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()310
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()323
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()332
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()422
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()422
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()428
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()428
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()428
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()428
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()440
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()524
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()621
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()621
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()621
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()642
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()642
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()655
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()734
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()812
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()812
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()844
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()856
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()856
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()1044
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()1134
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()1284
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1624
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1755
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()153871
+
+
+ + + +
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..b48f65d831 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,556 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()428
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()856
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()220
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()332
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()642
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()1284
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()1134
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1755
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()422
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()844
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler()4
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler()1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()812
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1624
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()734
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()1044
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()215
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()323
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()621
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()621
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()321
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()220
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()440
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()655
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()153871
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()31
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()62
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()107
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()214
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()428
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()856
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()428
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()112
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()642
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()621
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()422
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler_impl()1
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()812
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()310
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()108
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()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()107
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()220
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()524
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()31
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()107
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()107
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()428
+
+
+ + + +
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..2aef45feea --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines PublisherHandler and related convenience classes for upgrading the ROS publisher
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef PUBLISHER_HANDLER_H
+       6             : #define PUBLISHER_HANDLER_H
+       7             : 
+       8             : #include <ros/ros.h>
+       9             : #include <ros/package.h>
+      10             : 
+      11             : #include <atomic>
+      12             : #include <string>
+      13             : #include <mutex>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             : 
+      18             : /* class PublisherHandler_impl //{ */
+      19             : 
+      20             : /**
+      21             :  * @brief implementation of the publisher handler
+      22             :  */
+      23             : template <class TopicType>
+      24             : class PublisherHandler_impl {
+      25             : 
+      26             : public:
+      27             :   /**
+      28             :    * @brief default constructor
+      29             :    */
+      30             :   PublisherHandler_impl(void);
+      31             : 
+      32             :   /**
+      33             :    * @brief default destructor
+      34             :    */
+      35        7231 :   ~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        9989 :   PublisherHandler(void){};
+     102             : 
+     103             :   /**
+     104             :    * @brief generic destructor
+     105             :    */
+     106      169912 :   ~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         252 :   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..be57a05c04 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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)91982
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)128470
+
+
+ + + +
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..4afadba0a2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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)91982
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)128470
+
+
+ + + +
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..4ccc3020c6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html @@ -0,0 +1,117 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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      128470 : double inline throttleToForce(const MotorParams_t motor_params, const double throttle) {
+      20             : 
+      21      128470 :   return motor_params.n_motors * pow((throttle - motor_params.B) / motor_params.A, 2);
+      22             : }
+      23             : 
+      24       91982 : double inline forceToThrottle(const MotorParams_t motor_params, const double force) {
+      25             : 
+      26       91982 :   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..a4e861abc6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-06-08 22:13:12Functions:204445.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addMeasurement<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::Repredictor(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)1
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t::info_t(ros::Time const&)2
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)2
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChange<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)95
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, double const&)100
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, int const&)100
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)102
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChangeWithNoise<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)214
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&)290
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&)290
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t> > > > const&)300
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t const&, ros::Time const&, ros::Time const&)580
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&)580
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)628
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&)2066
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5150
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)6888
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&)16149
+
+
+ + + +
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..9b36b0288d --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-06-08 22:13:12Functions:204445.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5150
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)16149
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&)6888
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)628
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&)2066
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&)290
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&)580
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&)95
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&)290
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&)214
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&)580
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)2
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<3, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addMeasurement<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<6, 2, 2>, false>::Repredictor(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<6, 2, 2> > const&, unsigned int)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html new file mode 100644 index 0000000000..d18e49adfb --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html new file mode 100644 index 0000000000..03e0c965cb --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html @@ -0,0 +1,637 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-06-08 22:13:12Functions:204445.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef REPREDICTOR_H
+       2             : #define REPREDICTOR_H
+       3             : 
+       4             : #include <Eigen/Dense>
+       5             : #include <boost/circular_buffer.hpp>
+       6             : #include <std_msgs/Time.h>
+       7             : #include <functional>
+       8             : #include <ros/ros.h>
+       9             : #include <mrs_lib/utils.h>
+      10             : 
+      11             : namespace mrs_lib
+      12             : {
+      13             :   /**
+      14             :    * \brief Implementation of the Repredictor for fusing measurements with variable delays.
+      15             :    *
+      16             :    * A standard state-space system model is assumed for the repredictor with system inputs and measurements,
+      17             :    * generated from the system state vector. The inputs and measurements may be delayed with varying durations (an older measurement may become available after
+      18             :    * a newer one). A typical use-case scenario is when you have one "fast" but imprecise sensor and one "slow" but precise sensor and you want to use them both
+      19             :    * to get a good prediction (eg. height from the Garmin LiDAR, which comes at 100Hz, and position from a SLAM, which comes at 10Hz with a long delay). If the
+      20             :    * slow sensor is significantly slower than the fast one, fusing its measurement at the time it arrives without taking into account the sensor's delay may
+      21             :    * significantly bias your latest estimate.
+      22             :    *
+      23             :    * To accomodate this, the Repredictor keeps a buffer of N last inputs and measurements (N is specified
+      24             :    * in the constructor). This buffer is then used to re-predict the desired state to a specific time, as
+      25             :    * requested by the user. Note that the re-prediction is evaluated in a lazy manner only when the user requests it,
+      26             :    * so it goes through the whole history buffer every time a prediction is requested.
+      27             :    *
+      28             :    * The Repredictor utilizes a fusion Model (specified as the template parameter), which should implement
+      29             :    * the predict() and correct() methods. This Model is used for fusing the system inputs and measurements
+      30             :    * as well as for predictions. Typically, this Model will be some kind of a Kalman Filter (LKF, UKF etc.).
+      31             :    * \note The Model should be able to accomodate predictions with varying time steps in order for
+      32             :    * the Repredictor to work correctly (see eg. the varstepLKF class).
+      33             :    *
+      34             :    * \tparam Model                  the prediction and correction model (eg. a Kalman Filter).
+      35             :    * \tparam disable_reprediction   if true, reprediction is disabled and the class will act like a dumb LKF (for evaluation purposes).
+      36             :    *
+      37             :    */
+      38             :   template <class Model, bool disable_reprediction=false>
+      39             :   class Repredictor
+      40             :   {
+      41             :   public:
+      42             :     /* states, inputs etc. definitions (typedefs, constants etc) //{ */
+      43             : 
+      44             :     using x_t = typename Model::x_t;                  /*!< \brief State vector type \f$n \times 1\f$ */
+      45             :     using u_t = typename Model::u_t;                  /*!< \brief Input vector type \f$m \times 1\f$ */
+      46             :     using z_t = typename Model::z_t;                  /*!< \brief Measurement vector type \f$p \times 1\f$ */
+      47             :     using P_t = typename Model::P_t;                  /*!< \brief State uncertainty covariance matrix type \f$n \times n\f$ */
+      48             :     using R_t = typename Model::R_t;                  /*!< \brief Measurement noise covariance matrix type \f$p \times p\f$ */
+      49             :     using Q_t = typename Model::Q_t;                  /*!< \brief Process noise covariance matrix type \f$n \times n\f$ */
+      50             :     using statecov_t = typename Model::statecov_t;    /*!< \brief Helper struct for passing around the state and its covariance in one variable */
+      51             :     using ModelPtr = typename std::shared_ptr<Model>; /*!< \brief Shorthand type for a shared pointer-to-Model */
+      52             : 
+      53             :     //}
+      54             : 
+      55             :     /* predictTo() method //{ */
+      56             :     /*!
+      57             :      * \brief Estimates the system state and covariance matrix at the specified time.
+      58             :      *
+      59             :      * The measurement and system input histories are used to estimate the state vector and
+      60             :      * covariance matrix values at the specified time, which are returned.
+      61             :      *
+      62             :      * \param to_stamp   The desired time at which the state vector and covariance matrix should be estimated.
+      63             :      * \return           Returns the estimated state vector and covariance matrix in a single struct.
+      64             :      *
+      65             :      */
+      66             :     template<bool check=disable_reprediction>
+      67         102 :     std::enable_if_t<!check, statecov_t> predictTo(const ros::Time& to_stamp)
+      68             :     {
+      69         102 :       assert(!m_history.empty());
+      70         102 :       auto hist_it = std::begin(m_history);
+      71         102 :       if (hist_it->is_measurement)
+      72             :       {
+      73             :         // if the first history point is measurement, don't use it for correction (to prevent it from being used twice)
+      74           0 :         hist_it->is_measurement = false;
+      75             :       }
+      76             :       // cur_stamp corresponds to the time point of cur_sc estimation
+      77         102 :       auto cur_stamp = hist_it->stamp;
+      78             :       // cur_sc is the current state and covariance estimate
+      79         102 :       auto cur_sc = m_sc;
+      80       16047 :       do
+      81             :       {
+      82       16149 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       16149 :         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       16149 :         ros::Time next_stamp = to_stamp;
+      90       16149 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       16047 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       16149 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       16149 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       16149 :         hist_it++;
+      99       16149 :       } 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         580 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         580 :       assert(!m_history.empty());
+     120         580 :       const auto& info = m_history.front();
+     121         580 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         580 :       sc.stamp = to_stamp;
+     123         580 :       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        6988 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        6788 :           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         214 :     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         214 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         214 :       m_history.front().u = u;
+     172         214 :       m_history.front().Q = Q;
+     173         214 :       m_history.front().stamp = stamp;
+     174         214 :       m_history.front().predict_model = model;
+     175         214 :     }
+     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          95 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222          95 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224          95 :       m_history.front().u = u;
+     225          95 :       m_history.front().stamp = stamp;
+     226          95 :       m_history.front().predict_model = model;
+     227          95 :     }
+     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         290 :     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         290 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         290 :       auto& info = m_history.front();
+     327         290 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         290 :       const auto sc = predictTo(to_stamp);
+     329         290 :       info.z = z;
+     330         290 :       info.R = R;
+     331         290 :       info.stamp = to_stamp;
+     332         290 :       info.is_measurement = true;
+     333         290 :       info.meas_id = meas_id;
+     334         290 :       info.correct_model = model;
+     335         290 :       m_sc = correctFrom(sc, info);
+     336         290 :     }
+     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         630 :       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        6888 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        6888 :         u = info.u;
+     433        6888 :         Q = info.Q;
+     434        6888 :         predict_model = info.predict_model;
+     435        6888 :       };
+     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        2066 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2066 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       16729 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       33458 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       16729 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       33458 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5440 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5440 :       assert(meas.is_measurement);
+     544       10880 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5440 :       auto sc_tmp = sc;
+     546       10880 :       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..0b0094d867 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-06-08 22:13:12Functions: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..11900f1f2f --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-06-08 22:13:12Functions: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..1481b25005 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-06-08 22:13:12Functions: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..6b266616b6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-06-08 22:13:12Functions: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..8f6097cadc --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-06-08 22:13:12Functions: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..96d72d834d --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-06-08 22:13:12Functions: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..d12dcb733e --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-06-08 22:13:12Functions: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..02a0a97919 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-06-08 22:13:12Functions: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..eeb0f40bc6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-06-08 22:13:12Functions: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..9f135e4799 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-06-08 22:13:12Functions: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..882f71fe93 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-06-08 22:13:12Functions: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..5fe87aa853 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html @@ -0,0 +1,121 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-06-08 22:13:12Functions: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..11e45fefc0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-06-08 22:13:12Functions: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&)5629473
+
+
+ + + +
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..eae278eef0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-06-08 22:13:12Functions: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&)5629473
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..5479b9f35d --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.html @@ -0,0 +1,248 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-06-08 22:13:12Functions: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     5629473 :     time_point(const std::string& label) : ros_time(ros::Time::now()), chrono_time(std::chrono::steady_clock::now()), label(label) {
+      93     5628932 :     }
+      94             : 
+      95             :     time_point(const std::string& label, const ros::Time& ros_time) : ros_time(ros_time), label(label) {
+      96             :       // helper types to make the code slightly more readable
+      97             :       using float_seconds   = std::chrono::duration<float>;
+      98             :       using chrono_duration = std::chrono::steady_clock::duration;
+      99             :       // prepare ros and chrono current times to minimize their difference
+     100             :       const auto ros_now    = ros::Time::now();
+     101             :       const auto chrono_now = std::chrono::steady_clock::now();
+     102             :       // calculate how old is ros_time
+     103             :       const auto ros_dt = ros_now - ros_time;
+     104             :       // cast ros_dt to chrono type
+     105             :       const auto chrono_dt = std::chrono::duration_cast<chrono_duration>(float_seconds(ros_dt.toSec()));
+     106             :       // calculate ros_time in chrono time and set it to chrono_time
+     107             :       chrono_time = chrono_now - chrono_dt;
+     108             :     }
+     109             : 
+     110             :     ros::Time   ros_time;
+     111             :     chrono_tp   chrono_time;
+     112             :     std::string label;
+     113             :   };
+     114             :   //}
+     115             : 
+     116             : public:
+     117             :   /**
+     118             :    * @brief The basic constructor with a user-defined label of the timer, throttled period and file logger.
+     119             :    */
+     120             :   ScopeTimer(const std::string& label, const ros::Duration& throttle_period = ros::Duration(0), const bool enable = true,
+     121             :              const std::shared_ptr<ScopeTimerLogger> scope_timer_logger = nullptr);
+     122             : 
+     123             :   /**
+     124             :    * @brief The basic constructor with a user-defined label of the timer and a pre-start time, which will also be measured.
+     125             :    */
+     126             :   ScopeTimer(const std::string& label, const time_point& tp0, const ros::Duration& throttle_period = ros::Duration(0), const bool enable = true,
+     127             :              const std::shared_ptr<ScopeTimerLogger> scope_timer_logger = nullptr);
+     128             : 
+     129             :   /**
+     130             :    * @brief The basic constructor with a user-defined label of the timer and file logger.
+     131             :    */
+     132             :   ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable = true);
+     133             : 
+     134             :   /**
+     135             :    * @brief Checkpoint, prints the time passed until the point this function is called.
+     136             :    */
+     137             :   void checkpoint(const std::string& label);
+     138             : 
+     139             :   /**
+     140             :    * @brief Getter for scope timer lifetime
+     141             :    *
+     142             :    * @return lifetime as floating point milliseconds
+     143             :    */
+     144             :   float getLifetime();
+     145             : 
+     146             :   /**
+     147             :    * @brief The basic destructor which prints out or logs the scope times, if enabled.
+     148             :    */
+     149             :   ~ScopeTimer();
+     150             : 
+     151             : private:
+     152             :   std::string             _timer_label_;
+     153             :   bool                    _enable_print_or_log;
+     154             :   ros::Duration           _throttle_period_;
+     155             :   std::vector<time_point> checkpoints;
+     156             : 
+     157             :   std::shared_ptr<ScopeTimerLogger> _logger_ = nullptr;
+     158             : 
+     159             :   static std::unordered_map<std::string, ros::Time> last_print_times;
+     160             : };
+     161             : 
+     162             : }  // namespace mrs_lib
+     163             : 
+     164             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html new file mode 100644 index 0000000000..82f8580772 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html @@ -0,0 +1,61 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler()1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler_impl()1
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler()2
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler()2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::~ServiceClientHandler()4
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()107
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()109
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()214
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()214
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()214
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()214
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()218
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()428
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()644
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()644
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()1172
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()1172
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()1288
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()2237
+
+
+ + + +
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..cd5acca873 --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-06-08 22:13:12Functions:2424100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::ServiceClientHandler()2
mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>::~ServiceClientHandler()4
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::ServiceClientHandler()1
mrs_lib::ServiceClientHandler<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler()2
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()109
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()218
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()107
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()214
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()107
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()214
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()214
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()428
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()644
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()1288
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()1172
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()2237
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Float64Srv>::~ServiceClientHandler_impl()2
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ConstraintsOverride>::~ServiceClientHandler_impl()1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()107
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()214
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()644
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()1172
+
+
+ + + +
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..51ea6b875f --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html @@ -0,0 +1,327 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-06-08 22:13:12Functions:2424100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines ServiceClientHandler and related convenience classes for upgrading the ROS service client
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef SERVICE_CLIENT_HANDLER_H
+       6             : #define SERVICE_CLIENT_HANDLER_H
+       7             : 
+       8             : #include <ros/ros.h>
+       9             : #include <ros/package.h>
+      10             : 
+      11             : #include <string>
+      12             : #include <future>
+      13             : #include <mutex>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             : 
+      18             : /* class ServiceClientHandler_impl //{ */
+      19             : 
+      20             : /**
+      21             :  * @brief implementation of the service client handler
+      22             :  */
+      23             : template <class ServiceType>
+      24             : class ServiceClientHandler_impl {
+      25             : 
+      26             : public:
+      27             :   /**
+      28             :    * @brief default constructor
+      29             :    */
+      30             :   ServiceClientHandler_impl(void);
+      31             : 
+      32             :   /**
+      33             :    * @brief default destructor
+      34             :    */
+      35        2356 :   ~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        2356 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139        4605 :   ~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..e73f1f59b0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,3056 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-06-08 22:13:12Functions:39374452.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::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<Tester>(mrs_lib::SubscribeHandlerOptions const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const3
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)9
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getMsg()22
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()96
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const97
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const103
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*)104
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*)104
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()() const104
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*)107
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*)107
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*)107
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*)107
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&)107
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*)107
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*)107
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*)107
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*)107
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&)107
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*)107
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*)107
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*)107
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*)107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
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*)107
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*)107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const107
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()() const107
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const107
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()() const107
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>*)108
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>*)108
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>*)108
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>*)108
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&)108
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const108
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const108
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()() const108
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()() const108
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()() const108
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&)109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)109
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&)111
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()() const111
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&)112
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>*)112
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*)112
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>*)112
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*)112
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*)112
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*)112
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*)112
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*)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)112
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()() const112
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()() const112
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()() const112
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()() const112
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()() const112
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()() const112
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()113
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*)113
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*)113
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()() const113
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const122
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const133
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>*)176
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>*)176
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()() const176
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()207
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&)207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)207
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const211
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()214
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&)214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()214
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&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()214
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&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)214
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()214
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()214
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)214
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()() const214
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()() const214
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()220
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()224
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()245
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&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)245
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const259
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)309
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const309
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()311
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&)321
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&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()321
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&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)321
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()() const321
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()() const321
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()329
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&)329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)329
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()340
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&)340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)340
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()352
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&)352
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()363
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()406
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()406
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()406
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()406
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()407
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()417
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()417
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)417
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()() const417
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()426
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&)426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)426
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()428
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&)428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)428
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()428
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()428
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()429
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()437
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()437
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()438
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)459
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()() const459
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()() const459
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::~SubscribeHandler()468
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()490
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()513
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()516
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()522
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()597
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()630
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()640
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&)640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)640
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()642
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()644
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()710
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const710
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()834
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()842
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()855
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()856
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()918
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()918
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()970
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()1171
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1453
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()2093
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2655
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2766
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()3978
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()4557
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()14591
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const17662
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const18480
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const18697
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const18709
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()19454
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const37905
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const39907
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()51221
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const57883
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const72004
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()72526
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const85363
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const117754
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const147862
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()179465
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const179470
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()197055
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const227722
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()246332
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const265252
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const407920
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()428694
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const453425
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const560419
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const572556
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()594895
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()764827
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const979707
+
+
+ + + +
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..1f53f8fe58 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,3056 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-06-08 22:13:12Functions:39374452.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()426
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&)426
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()429
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*)107
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*)107
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*)104
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>*)108
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*)107
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*)107
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*)104
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>*)108
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()855
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)426
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()437
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::~SubscribeHandler()468
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()214
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&)214
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()214
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&)107
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*)107
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*)107
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()428
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)214
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()207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()14591
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&)207
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()437
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)176
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>*)176
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()644
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)207
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()406
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()406
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()113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()4557
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&)109
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()407
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)108
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)108
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()516
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)109
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()340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()197055
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&)340
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()630
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&)112
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>*)112
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)112
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>*)112
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::HdgPassthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)112
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()970
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)340
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()640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()764827
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&)640
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1453
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)309
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)107
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*)112
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*)112
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*)107
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*)112
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*)112
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()2093
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)640
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()406
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()406
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()245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()311
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&)245
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()245
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&)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()490
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)245
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()218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()179465
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()220
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&)111
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()438
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)218
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()417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()428694
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()417
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&)417
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()834
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)417
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()428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()51221
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&)428
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()428
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&)321
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()856
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)428
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()214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()792
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const>)> const&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()214
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&)214
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()428
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)214
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()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()112
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&)112
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)112
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()107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()710
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)107
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()107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()363
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)107
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()352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()19454
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&)352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()245
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&)321
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()597
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)245
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getMsg()22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)107
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)107
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()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)107
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()107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()5
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)107
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()459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()72526
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()918
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)459
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)31
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_autostart::automatic_start::AutomaticStart>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)31
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()62
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)107
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()459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()594895
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()459
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&)459
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()918
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)459
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()107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()107
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&)107
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)> const&)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()214
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()96
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()513
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::transform_manager::TransformManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()522
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()3978
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&)321
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()321
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
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*)107
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*)107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trajectory_generation::MrsTrajectoryGeneration>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()642
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)321
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()329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()246332
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&)329
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()842
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<Tester>(mrs_lib::SubscribeHandlerOptions const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)108
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)107
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*)113
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*)107
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*)113
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()1171
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)329
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()214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()39907
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()214
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&)214
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()428
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)214
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2766
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const9688
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const18697
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() const17662
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() const407920
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() const979707
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]() const133
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() const211
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const18709
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() const179470
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() const453425
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const117754
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const560419
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() const72004
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const710
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() const259
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2655
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() const37905
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]() const108
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const108
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() const85363
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() const18480
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() const103
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const147862
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const122
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const103
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const97
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() const227722
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const265252
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() const572556
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const39907
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const57883
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()() const107
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()() const107
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()() const104
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()() const108
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const107
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()() const107
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const176
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const108
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()() const112
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()() const112
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::HdgPassthrough::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::HdgPassthrough*)::{lambda()#1}::operator()() const112
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const309
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const107
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()() const112
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()() const112
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()() const107
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const111
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()() const107
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()() const417
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()() const321
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()() const107
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()() const214
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()() const112
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()() const107
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const107
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()() const321
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const107
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()() const107
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()() const107
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()() const107
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()() const459
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const107
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const31
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const107
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()() const459
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const107
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::transform_manager::TransformManager::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_managers::transform_manager::TransformManager*)::{lambda()#1}::operator()() const3
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const107
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()() const107
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trajectory_generation::MrsTrajectoryGeneration::*)(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>), mrs_uav_trajectory_generation::MrsTrajectoryGeneration*)::{lambda()#1}::operator()() const107
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()() const108
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)::{lambda()#1}::operator()() const107
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()() const113
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const214
+
+
+ + + +
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..d056baf564 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html @@ -0,0 +1,563 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-06-08 22:13:12Functions:39374452.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        2766 :     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     2629495 :       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     3010504 :       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      560419 :       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             :       * \param timeout    after this duration, this method will return a nullptr if no new data becomes available.
+     140             :       * \return           the message if a new message is available after waking up, \p nullptr otherwise.
+     141           0 :       */
+     142             :       virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout) {assert(m_pimpl); return m_pimpl->waitForNew(timeout);};
+     143             : 
+     144             :     /*!
+     145             :       * \brief Returns time of the last received message on the topic, handled by this SubscribeHandler.
+     146             :       *
+     147             :       * \return time when the last message was received.
+     148      721735 :       */
+     149             :       virtual ros::Time lastMsgTime() const {assert(m_pimpl); return m_pimpl->lastMsgTime();};
+     150             : 
+     151             :     /*!
+     152             :       * \brief Returns the resolved (full) name of the topic, handled by this SubscribeHandler.
+     153             :       *
+     154             :       * \return name of the handled topic.
+     155         364 :       */
+     156             :       virtual std::string topicName() const {assert(m_pimpl); return m_pimpl->topicName();};
+     157             : 
+     158             :     /*!
+     159             :       * \brief Returns the subscribed (unresolved) name of the topic, handled by this SubscribeHandler.
+     160             :       *
+     161             :       * \return name of the handled topic.
+     162           0 :       */
+     163             :       virtual std::string subscribedTopicName() const {assert(m_pimpl); return m_pimpl->m_topic_name;};
+     164             : 
+     165             :     /*!
+     166             :       * \brief Returns number of publishers registered at the topic.
+     167             :       *
+     168             :       * \return number of publishers.
+     169             :       */
+     170        7007 :       virtual uint32_t getNumPublishers() const {assert(m_pimpl); return m_pimpl->getNumPublishers();};
+     171             : 
+     172             :     /*!
+     173             :       * \brief Sets the timeout for no received message.
+     174             :       *
+     175             :       * \param timeout    The new timeout for no received messages.
+     176             :       */
+     177             :       virtual void setNoMessageTimeout(const ros::Duration& timeout) {assert(m_pimpl); return m_pimpl->setNoMessageTimeout(timeout);};
+     178           5 : 
+     179             :     /*!
+     180             :       * \brief Enables the callbacks for the handled topic.
+     181             :       *
+     182             :       * If the SubscribeHandler object is stopped using the stop() method, no callbacks will be called
+     183             :       * until the start() method is called.
+     184             :       */
+     185             :       virtual void start() {assert(m_pimpl); return m_pimpl->start();};
+     186             : 
+     187       10747 :     /*!
+     188             :       * \brief Disables the callbacks for the handled topic.
+     189             :       *
+     190             :       * All messages after this method is called will be ignored until start() is called again.
+     191             :       * Timeout checking will also be disabled.
+     192             :       */
+     193             :       virtual void stop() {assert(m_pimpl); return m_pimpl->stop();};
+     194             : 
+     195             :     public:
+     196             :     /*!
+     197             :       * \brief Default constructor to avoid having to use pointers.
+     198        7003 :       *
+     199             :       * It does nothing and the SubscribeHandler it constructs will also do nothing.
+     200             :       * Use some of the other constructors for actual construction of a usable SubscribeHandler.
+     201             :       */
+     202             :       SubscribeHandler() {};
+     203             : 
+     204             :     /*!
+     205        7003 :       * \brief Main constructor.
+     206             :       *
+     207       14006 :       * \param options    The common options struct (see documentation of SubscribeHandlerOptions).
+     208        7003 :       * \param topic_name Name of the topic to be handled by this subscribed.
+     209       14006 :       * \param args       Remaining arguments to be parsed (see other constructors).
+     210             :       *
+     211             :       */
+     212        7003 :       template<class ... Types>
+     213             :       SubscribeHandler(
+     214        7003 :             const SubscribeHandlerOptions& options,
+     215             :             const std::string& topic_name,
+     216             :             Types ... args
+     217             :           )
+     218             :       :
+     219             :         SubscribeHandler(
+     220             :             [options, topic_name]()
+     221             :             {
+     222             :               SubscribeHandlerOptions opts = options;
+     223        7003 :               opts.topic_name = topic_name;
+     224             :               return opts;
+     225             :             }(),
+     226             :             args...
+     227        7003 :             )
+     228        7003 :       {
+     229             :       }
+     230        7003 : 
+     231             :     /*!
+     232             :       * \brief Convenience constructor overload.
+     233             :       *
+     234             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     235             :       * \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).
+     236             :       *
+     237             :       */
+     238           0 :       SubscribeHandler(
+     239             :             const SubscribeHandlerOptions& options,
+     240             :             const message_callback_t& message_callback = {}
+     241             :           )
+     242             :       {
+     243             :         if (options.threadsafe)
+     244        7003 :         {
+     245        7002 :           m_pimpl = std::make_unique<ImplThreadsafe>
+     246        7003 :             (
+     247             :               options,
+     248             :               message_callback
+     249             :             );
+     250             :         }
+     251             :         else
+     252             :         {
+     253             :           m_pimpl = std::make_unique<Impl>
+     254             :             (
+     255             :               options,
+     256             :               message_callback
+     257           1 :             );
+     258             :         }
+     259             :         if (options.autostart)
+     260             :           start();
+     261             :       };
+     262             : 
+     263             :     /*!
+     264           1 :       * \brief Convenience constructor overload.
+     265             :       *
+     266           2 :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     267           1 :       * \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.
+     268           2 :       * \param args             Remaining arguments to be parsed (see other constructors).
+     269             :       *
+     270             :       */
+     271           1 :       template <class ... Types>
+     272             :       SubscribeHandler(
+     273           1 :             const SubscribeHandlerOptions& options,
+     274             :             const timeout_callback_t& timeout_callback,
+     275             :             Types ... args
+     276             :           )
+     277             :       :
+     278             :         SubscribeHandler(
+     279             :             [options, timeout_callback]()
+     280             :             {
+     281             :               SubscribeHandlerOptions opts = options;
+     282             :               opts.timeout_callback = timeout_callback;
+     283             :               return opts;
+     284             :             }(),
+     285             :             args...
+     286             :             )
+     287             :       {
+     288             :       }
+     289             : 
+     290             :     /*!
+     291             :       * \brief Convenience constructor overload.
+     292             :       *
+     293             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     294             :       * \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.
+     295             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     296             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     297             :       *
+     298             :       */
+     299             :       template <class ObjectType1, class ... Types>
+     300             :       SubscribeHandler(
+     301             :             const SubscribeHandlerOptions& options,
+     302             :             void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     303             :             ObjectType1* const obj1,
+     304             :             Types ... args
+     305             :           )
+     306             :       :
+     307             :         SubscribeHandler(
+     308             :             [options, timeout_callback, obj1]()
+     309             :             {
+     310             :               SubscribeHandlerOptions opts = options;
+     311             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     312             :               return opts;
+     313             :             }(),
+     314        3003 :             args...
+     315             :             )
+     316             :       {
+     317             :       }
+     318             : 
+     319             :     /*!
+     320             :       * \brief Convenience constructor overload.
+     321             :       *
+     322             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     323        6006 :       * \param message_callback The callback method to call when a new message is received.
+     324             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     325        3003 :       * \param args             Remaining arguments to be parsed (see other constructors).
+     326             :       *
+     327        3003 :       */
+     328             :       template <class ObjectType2, class ... Types>
+     329             :       SubscribeHandler(
+     330             :             const SubscribeHandlerOptions& options,
+     331             :             void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     332             :             ObjectType2* const obj2,
+     333             :             Types ... args
+     334             :           )
+     335             :       :
+     336             :         SubscribeHandler(
+     337             :             options,
+     338             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     339             :             args...
+     340             :             )
+     341             :       {
+     342             :       }
+     343             : 
+     344             :     /*!
+     345             :       * \brief Convenience constructor overload.
+     346             :       *
+     347             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     348             :       * \param message_callback The callback method to call when a new message is received.
+     349             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     350             :       * \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.
+     351             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     352             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     353             :       *
+     354             :       */
+     355             :      template <class ObjectType1, class ObjectType2, class ... Types>
+     356             :      SubscribeHandler(
+     357             :            const SubscribeHandlerOptions& options,
+     358             :            void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     359             :            ObjectType2* const obj2,
+     360             :            void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     361             :            ObjectType1* const obj1,
+     362             :            Types ... args
+     363             :          )
+     364             :      :
+     365             :        SubscribeHandler(
+     366             :             [options, timeout_callback, obj1]()
+     367             :             {
+     368             :               SubscribeHandlerOptions opts = options;
+     369             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     370             :               return opts;
+     371             :             }(),
+     372             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     373             :             args...
+     374             :             )
+     375             :      {
+     376             :      }
+     377             : 
+     378             :     /*!
+     379             :       * \brief Convenience constructor overload.
+     380             :       *
+     381             :       * \param options            The common options struct (see documentation of SubscribeHandlerOptions).
+     382             :       * \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.
+     383             :       * \param args               Remaining arguments to be parsed (see other constructors).
+     384             :       *
+     385             :       */
+     386             :       template<class ... Types>
+     387             :       SubscribeHandler(
+     388             :             const SubscribeHandlerOptions& options,
+     389             :             const ros::Duration& no_message_timeout,
+     390             :             Types ... args
+     391             :           )
+     392             :       :
+     393             :         SubscribeHandler(
+     394             :             [options, no_message_timeout]()
+     395             :             {
+     396             :               SubscribeHandlerOptions opts = options;
+     397             :               opts.no_message_timeout = no_message_timeout;
+     398             :               return opts;
+     399             :             }(),
+     400             :             args...
+     401             :             )
+     402             :       {
+     403             :       }
+     404             : 
+     405             :     /*!
+     406             :       * \brief Convenience constructor overload.
+     407             :       *
+     408             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     409             :       * \param timeout_manager  The manager for timeout callbacks.
+     410             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     411             :       *
+     412             :       */
+     413       17770 :       template <class ... Types>
+     414             :       SubscribeHandler(
+     415             :             const SubscribeHandlerOptions& options,
+     416             :             mrs_lib::TimeoutManager& timeout_manager,
+     417             :             Types ... args
+     418          20 :           )
+     419          20 :       :
+     420          20 :         SubscribeHandler(
+     421          20 :             options,
+     422          20 :             timeout_manager = timeout_manager,
+     423        6876 :             args...
+     424             :             )
+     425        6876 :       {
+     426        6876 :       }
+     427        6876 : 
+     428             :       ~SubscribeHandler() = default;
+     429             :       // delete copy constructor and assignment operator (forbid copying shandlers)
+     430             :       SubscribeHandler(const SubscribeHandler&) = delete;
+     431             :       SubscribeHandler& operator=(const SubscribeHandler&) = delete;
+     432             :       // define only move constructor and assignemnt operator
+     433             :       SubscribeHandler(SubscribeHandler&& other)
+     434             :       {
+     435             :         this->m_pimpl = std::move(other.m_pimpl);
+     436             :         other.m_pimpl = nullptr;
+     437             :       }
+     438             :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     439             :       {
+     440             :         this->m_pimpl = std::move(other.m_pimpl);
+     441             :         other.m_pimpl = nullptr;
+     442             :         return *this;
+     443             :       }
+     444             : 
+     445             :     private:
+     446             :       class Impl;
+     447             :       class ImplThreadsafe;
+     448             :       std::unique_ptr<Impl> m_pimpl;
+     449             :   };
+     450             :   //}
+     451             : 
+     452             : /*!
+     453             :   * \brief Helper alias for convenient extraction of handled message type from a SubscribeHandlerPtr.
+     454             :   */
+     455             :   template<typename SubscribeHandler>
+     456             :   using message_type = typename SubscribeHandler::message_type;
+     457             : 
+     458             : /*!
+     459             :   * \brief Helper function for object construstion e.g. in case of member objects.
+     460             :   * This function is useful to avoid specifying object template parameters twice - once in definition of the variable and second time during object construction.
+     461             :   * 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
+     462             :   * argument and not as a return type.
+     463             :   *
+     464             :   * \param object The object to be constructed.
+     465             :   * \param args   These arguments are passed to the object constructor.
+     466             :   */
+     467             :   template<typename Class, class ... Types>
+     468             :   void construct_object(
+     469             :         Class& object,
+     470             :         Types ... args
+     471             :       )
+     472             :   {
+     473             :     object = Class(args...);
+     474             :   }
+     475             : }
+     476             : 
+     477             : #include <mrs_lib/impl/subscribe_handler.hpp>
+     478             : 
+     479             : #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..15111c902e --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html @@ -0,0 +1,140 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ 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..f6c46da64f7296f498959eedd73dc5a3c21f05f6 GIT binary patch literal 1968 zcmV;h2T%BkP)4pW4nq#=>b^G&sN-QMR) zn`&QH30X93krHrxLuAnL#;gf(1aubW3dA|&yD^R%AQO#y$YV_A;pVt&G^in&&`=+h zYX`9r!T>g(8S7&xJq6@TA4iU1(kJ#z^2h;>o3b=4A(p-!{5VoCWu#*)1Cp0xF*rJH zvkXT7uuHza1W>?sQ-?1rB#(^jPK*{8M9yFzq-5e~l2I3NIfjE8TMXiX?J}EFCX^Bk zyUB)^A4b9)bfQ=_5tE=8a-N~6v3*i>!mbv|n6(zHP#3Po%$$v zu#2*XVc5H_j~UTT^e#C!IhsXdMj-LwI7aT80;0ADRn7#4*aeTL0uVvHN>C+)6J@NT zr>?w$(BrUN+(A@ch#{9Ft^**{JDee8W$08!*+eT(`jruVv~LWg;ipEXs?zhy=%Uu8 z%Zhhr{{C6}v= z64*4I)`JF9+KfyXUuivm@EGACV5r8GXy%89fFbTf@UU|o{Rw?8w^ursD%4f!!3oa| zPhP53bH^x^Ou%*A2{;8%&QN7F1-}H3N$XCKx@#b(Tpvpr#VL`Py zm&s>lNpUN?U&R&8*i54-E=Ev5alHV3GXsVf_PAghfk)oI2_C6z3y2b-hvPXsBhIB` z(wDmXN)>>kjliSk%;H;$(8H0EK(~Y*uODmn^V$?gNxxb2jl}Q@TgApRXQyO94R2&Z z747gm@Lr8L?wyO)#KK|N9q^RfC$v3M%#l#E+IW$KcZA1}BV!pjV010H+RQjUUSz~8 zWs!Wb7P}xxYpZlMffR&jl1K$xCS*I|%Bw2Hk0V0|8dLv@iTR z2p1f)Rf5y9;%s-#*I6_t4^Bn`R!A$EX=GfBcF4&3$O%Imj%>K7ZxeBSpBh{Xhe5@8X$0nen;p`n=Fe zXlg?>Me$x!e4yl-a6-%#FQuh?q)F!~wOIg|8~L2n)f+E)3Xn_vv$;Z|Rr7>A58pjT z(slL1N#b~X>w3SIYP9{(Y_zj%X5xRURqfH{3dplRg=KxPo$76a$8J8>IN{r66RWuH zkdK+LPmBX&8$y%yB)R4tAfqampQ!MbT(|PQ2G2mrHMgs-QZ!Fh7Ad%vg(u=ta?S0k z>vbj9Lfz|W{o0c2rmhN78t>m#a`hj$vgA4to}5d~IocxG-bI@}S5kQht!->G{zsHt zWyKYQkhXthmq|YokA!5xpiX>vw_UuaXLwqVGy6KB(Fuzp4#8@L{_{$zl>Fw1YaT+^ z535Y=$3)y#$Sp-j4q|jpf=G|?u@(1Ree~|TwkCN8!mBGOP&6Gc|1AZQ(!2e{OJk4r z|N5-`Z>g!;eoRQQ7IAbg7TE3%9N%9_&6lrUlM3dLQc^Wt(GXHn2Mn`U-)^%-ZCN5Y znG-9*o8DdQ5MH+EVxsaAnwqwr&M)n84M6*mK0+JRYiBx=wrfSCipj-il+)`-NTLDh zpkcjn-{9GhLO*#g+@`hrz=S=9jve1UETr}5iOTDhnW`PCwCnTCSj@1|LuhHVC9uv>xlu9#^r} zYhfBJbl*Cg*)It^#qsjsSosT8gFdA>s=wdRR{I}fv1+LH?bHbX0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..3a7703bfaa --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.func.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..8adc4ea07a --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html @@ -0,0 +1,169 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions:00-
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/timer.h.gcov.png b/mrs_lib/include/mrs_lib/timer.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..97b93a280113be0ff271b38b8b525019ab2a17eb GIT binary patch literal 930 zcmV;T16}-yP)k3Lcs7^xQ&-t(7ayC zAgVM99$F?rhv|ce%%;sNWFQYU+D)3h14y=k!^ga-&ksg zsLd%MWWkU%B1=S-X#f-}b*BwR z3!VhVm&T{~;lc1&m&`ApI^lvipj!@4S^l|)6?co$-Y;f{mVw3@e1@#S+k1{9N~X)- zXcCHDi)O6sV<{LVWNbzys2T>g&xSlIR2rkf5(I9|rJ%MOV&<@E+zS%hNWnNdt`LdQ zL)D3;1|VPt!ZGnrh=l+S&iR39_tbpU{yv{(g@4rli>$cPtV}bS3sjMQI?did&Q|s4 zpFz%!>d>n3B4-yl>-l(*v#!SXk+U_0&A{^M*YPr+pm~;#AJ}+;$=N+oh#o!Lv$!5j zDao0-4r~UqV{xZCxgBGju4gP(8g=q_B3WckYh21uqQ>I>7V6qYzd>S}v7fEErkHuJ zK>x{IJx~JrnWj|K1R%9W7bRVlvq|G`rKEDI=gblxE5VDBD#4=b0v9GvNGc{7OQ8^m zUyZ)8u0NP&7bWcrcG67daZuuma%_7GCAFFckYNvlj;2bLzeRGbrh$`yM)ubW@5ZF$ zdv}gB-?DB>Atm@lbi_5c6? literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html b/mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html new file mode 100644 index 0000000000..df54a9fa84 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-06-08 22:13:12Functions:141877.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
std::optional<boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > > mrs_lib::Transformer::transform<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
std::optional<boost::shared_ptr<geometry_msgs::Quaternion_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::shared_ptr<geometry_msgs::Quaternion_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
std::optional<boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > > mrs_lib::Transformer::transformSingle<geometry_msgs::PointStamped_<std::allocator<void> > >(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
mrs_lib::Transformer::inverse(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)333
mrs_lib::Transformer::retryLookupNewest(bool)536
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1518
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)488227
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2105189
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&)2105244
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2108075
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)2118647
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)2118658
+
+
+ + + +
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..150b0d7a41 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-06-08 22:13:12Functions: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&)2118647
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2105189
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1518
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)488227
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&)2105244
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)333
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)536
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&)2118658
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2108075
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..3d0d75de08 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.html @@ -0,0 +1,764 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-06-08 22:13:12Functions: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      488227 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132      976029 :       std::scoped_lock lck(mutex_);
+     133      488086 :       default_frame_id_ = frame_id;
+     134      488001 :     }
+     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         333 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155         666 :       std::scoped_lock lck(mutex_);
+     156         333 :       if (prefix.empty())
+     157           1 :         prefix_ = "";
+     158             :       else
+     159         332 :         prefix_ = prefix + "/";
+     160         333 :     }
+     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         536 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213         536 :       std::scoped_lock lck(mutex_);
+     214         536 :       retry_lookup_newest_ = retry;
+     215         536 :     }
+     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        1518 :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250        3036 :       std::scoped_lock lck(mutex_);
+     251        3036 :       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     2118647 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552     2118647 :       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     2105189 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565     2105189 :       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     2118658 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576     2118658 :       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     2108075 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589     2108075 :       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     2105244 :     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     2105244 :       geometry_msgs::TransformStamped ret;
+     621     2105128 :       frame_from(ret) = from_frame;
+     622     2105232 :       frame_to(ret) = to_frame;
+     623     2105207 :       ret.header.stamp = time_stamp;
+     624     2105207 :       ret.transform = tf;
+     625     2105207 :       return ret;
+     626             :     }
+     627             :     //}
+     628             : 
+     629             :     /* copyChangeFrame() and related methods //{ */
+     630             : 
+     631             :     // helper type and member for detecting whether a message has a header using SFINAE
+     632             :     template<typename T>
+     633             :     using has_header_member_chk = decltype( std::declval<T&>().header );
+     634             :     template<typename T>
+     635             :     static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
+     636             :     
+     637             :     template <typename msg_t>
+     638             :     std_msgs::Header getHeader(const msg_t& msg);
+     639             :     template <typename pt_t>
+     640             :     std_msgs::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
+     641             :     
+     642             :     template <typename msg_t>
+     643             :     void setHeader(msg_t& msg, const std_msgs::Header& header);
+     644             :     template <typename pt_t>
+     645             :     void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::Header& header);
+     646             :     
+     647             :     template <typename T>
+     648             :     T copyChangeFrame(const T& what, const std::string& frame_id);
+     649             :     
+     650             :     //}
+     651             : 
+     652             :     /* methods for converting between lattitude/longitude and UTM coordinates //{ */
+     653             :     geometry_msgs::Point LLtoUTM(const geometry_msgs::Point& what, const std::string& prefix);
+     654             :     geometry_msgs::PointStamped LLtoUTM(const geometry_msgs::PointStamped& what, const std::string& prefix);
+     655             :     geometry_msgs::Pose LLtoUTM(const geometry_msgs::Pose& what, const std::string& prefix);
+     656             :     geometry_msgs::PoseStamped LLtoUTM(const geometry_msgs::PoseStamped& what, const std::string& prefix);
+     657             :     
+     658             :     std::optional<geometry_msgs::Point> UTMtoLL(const geometry_msgs::Point& what, const std::string& prefix);
+     659             :     std::optional<geometry_msgs::PointStamped> UTMtoLL(const geometry_msgs::PointStamped& what, const std::string& prefix);
+     660             :     std::optional<geometry_msgs::Pose> UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix);
+     661             :     std::optional<geometry_msgs::PoseStamped> UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix);
+     662             :     
+     663             :     // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
+     664             :     template<class Class, typename Message>
+     665             :     using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
+     666             :     template<class Class, typename Message>
+     667             :     using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
+     668             :     template<class Class, typename Message>
+     669             :     static constexpr bool UTMLL_exists_v = std::experimental::is_detected<UTMLL_method_chk, Class, Message>::value && std::experimental::is_detected<LLUTM_method_chk, Class, Message>::value;
+     670             :     //}
+     671             : 
+     672             :   };
+     673             : 
+     674             :   //}
+     675             : 
+     676             : }  // namespace mrs_lib
+     677             : 
+     678             : #include <mrs_lib/impl/transformer.hpp>
+     679             : 
+     680             : #endif  // TRANSFORMER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html new file mode 100644 index 0000000000..5aceca5a90 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html @@ -0,0 +1,190 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.png b/mrs_lib/include/mrs_lib/visual_object.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2e10038ce016599658d08255c1ff1a58c48536b4 GIT binary patch literal 493 zcmV+saxyxDMeS`bZ>XVXLfMj6TyyoCqrtw2lm0u z+o(BT9)Ca$kB$N})=&j%ZYAN~M-2fl?xgk-O5E?%?$Wmj&*fS>a()0krI)3KDBPQ< zbwrxU^|#FxFTAkosWR(uv15M}_ueAI2C<4{orGw`5gG}@y*-=U|C$HgxkA&LlV~x;9avy^{ zD4UE5G>^=dcMGuSb&_NEgJF^XJ)xiRS!%!{##?Fl?i#WPsloKLN2!F_$XG*)sD*#d joDtA2469xit&98vAi`i14~K$;00000NkvXXu0mjfj#t*F literal 0 HcmV?d00001 diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html new file mode 100644 index 0000000000..acf1cbf1b1 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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&)23394
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)113662
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()148007
mrs_lib::AttitudeConverter::getYaw()201598
mrs_lib::AttitudeConverter::calculateRPY()201603
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)299119
mrs_lib::AttitudeConverter::setHeading(double const&)315053
mrs_lib::AttitudeConverter::getVectorZ()317051
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)322504
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const401567
mrs_lib::AttitudeConverter::operator tf2::Transform() const429118
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)495855
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const639558
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const943929
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)1000645
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1328017
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1620720
mrs_lib::AttitudeConverter::getHeading()1716351
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)3798497
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)8000313
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const9286564
mrs_lib::AttitudeConverter::validateOrientation()14621069
+
+
+ + + +
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..fe0d4a8c10 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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&)299119
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)23394
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()1716351
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()317051
mrs_lib::AttitudeConverter::setHeading(double const&)315053
mrs_lib::AttitudeConverter::calculateRPY()201603
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)322504
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)113662
mrs_lib::AttitudeConverter::validateOrientation()14621069
mrs_lib::AttitudeConverter::getYaw()201598
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> >)3798497
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)1000645
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)495855
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1328017
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&)8000313
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()148007
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>() const639558
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const9286564
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const943929
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const401567
mrs_lib::AttitudeConverter::operator tf2::Transform() const429118
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1620720
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..80e4c80e29 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html @@ -0,0 +1,561 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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       23394 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34       23394 :   vector3_[0] = vector3[0];
+      35       23394 :   vector3_[1] = vector3[1];
+      36       23394 :   vector3_[2] = vector3[2];
+      37       23394 : }
+      38             : 
+      39      299119 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41      299095 :   vector3_[0] = vector3.x;
+      42      299099 :   vector3_[1] = vector3.y;
+      43      299061 :   vector3_[2] = vector3.z;
+      44      299069 : }
+      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      639558 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60      639558 :   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     8000313 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80     7997714 :   switch (format) {
+      81             : 
+      82     7998199 :     case RPY_EXTRINSIC: {
+      83     7998199 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84     7998119 :       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     7997644 :   validateOrientation();
+     104     7999266 : }
+     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     3798497 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117     3798313 :   tf2_quaternion_.setX(quaternion.x);
+     118     3797828 :   tf2_quaternion_.setY(quaternion.y);
+     119     3798305 :   tf2_quaternion_.setZ(quaternion.z);
+     120     3798467 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122     3798560 :   validateOrientation();
+     123     3797655 : }
+     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      495855 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132      495857 :   tf2_quaternion_.setX(quaternion.x());
+     133      495856 :   tf2_quaternion_.setY(quaternion.y());
+     134      495856 :   tf2_quaternion_.setZ(quaternion.z());
+     135      495856 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137      495857 :   validateOrientation();
+     138      495856 : }
+     139             : 
+     140     1328017 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141     1325778 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143     1326434 :   tf2_quaternion_.setX(quaternion.x());
+     144     1324737 :   tf2_quaternion_.setY(quaternion.y());
+     145     1326155 :   tf2_quaternion_.setZ(quaternion.z());
+     146     1326167 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148     1326095 :   validateOrientation();
+     149     1323296 : }
+     150             : 
+     151     1000645 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153      998485 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155      998485 :   validateOrientation();
+     156      998286 : }
+     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      943929 : AttitudeConverter::operator tf2::Quaternion() const {
+     170      943929 :   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     9286564 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187     9286564 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189     9284323 :   geom_quaternion.x = tf2_quaternion_.x();
+     190     9278563 :   geom_quaternion.y = tf2_quaternion_.y();
+     191     9278738 :   geom_quaternion.z = tf2_quaternion_.z();
+     192     9280169 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194     9279355 :   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     1620720 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207     1620720 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209     3241072 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212      148007 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214      148007 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216      148007 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219      401567 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221      401567 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224      429118 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226      429118 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233      201598 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235      201598 :   calculateRPY();
+     236             : 
+     237      201598 :   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     1716351 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256     1716351 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258     1716016 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260     1715169 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           1 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264     3428009 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267      113662 : 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      113662 :   if (fabs(heading_rate) < 1e-3) {
+     272       82565 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276       31097 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279       31097 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280       31097 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283       31097 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284       31097 :   b_orb.normalize();
+     285       31097 :   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       31097 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291       31097 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292       31097 :   double projected_norm        = projected.norm();
+     293             : 
+     294       31097 :   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       31097 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301       31097 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303       31097 :   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       31097 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312      322504 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315      322504 :   Eigen::Matrix3d R = *this;
+     316      322491 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319      322455 :   Eigen::Matrix3d W;
+     320      322437 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323      322451 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326      322433 :   double rx = R(0, 0);  // x-component of body X
+     327      322381 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329      322418 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331      322418 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336      322418 :   double atan2_d_x = -ry / denom;
+     337      322418 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340      322418 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342      322438 :   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      317051 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365      317051 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367      317051 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369      634099 :   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      315053 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406      315053 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409      315051 :   if (fabs(b3[2]) < 1e-3) {
+     410           7 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414      315045 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416      315046 :   Eigen::Matrix3d new_R;
+     417             : 
+     418      315042 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421      315040 :   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      628712 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425      314795 :   A.col(0)          = projector_body_z_compl.col(0);
+     426      314743 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429      628357 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430      314790 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431      314643 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434      627936 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435      628315 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436      313995 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438      313879 :   new_R.col(0) = oblique_projector * h;
+     439      313223 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443      313633 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444      311779 :   new_R.col(1).normalize();
+     445             : 
+     446      626966 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455      201603 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457      201603 :   if (!got_rpy_) {
+     458             : 
+     459      201603 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461      201603 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467    14621069 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469    29232459 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470    14608190 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473    14610244 : }
+     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..658a2192c9 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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..024911d66c --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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..3359ed0331 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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..0bfd0fdfab --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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..4cb9ff2dc6 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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..d26544e7b9 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-06-08 22:13:12Functions: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..38cfcc1546 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-06-08 22:13:12Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::BatchVisualizer::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addTrajectory(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setLinesScale(double)0
mrs_lib::BatchVisualizer::addRay(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCone(mrs_lib::geometry::Cone const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addPath(mrs_msgs::Path_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCuboid(mrs_lib::geometry::Cuboid const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setParentFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)22
mrs_lib::BatchVisualizer::setPointsScale(double)22
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)79
mrs_lib::BatchVisualizer::initialize()214
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> >)214
mrs_lib::BatchVisualizer::BatchVisualizer()214
mrs_lib::BatchVisualizer::clearBuffers()236
mrs_lib::BatchVisualizer::clearVisuals()236
mrs_lib::BatchVisualizer::~BatchVisualizer()428
mrs_lib::BatchVisualizer::addNullPoint()450
mrs_lib::BatchVisualizer::addNullLine()462
mrs_lib::BatchVisualizer::addNullTriangle()462
mrs_lib::BatchVisualizer::publish()462
+
+
+ + + +
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..ae767b7ba0 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-06-08 22:13:12Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::BatchVisualizer::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::initialize()214
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullLine()462
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullPoint()450
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::clearBuffers()236
mrs_lib::BatchVisualizer::clearVisuals()236
mrs_lib::BatchVisualizer::addTrajectory(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setLinesScale(double)0
mrs_lib::BatchVisualizer::setParentFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)22
mrs_lib::BatchVisualizer::setPointsScale(double)22
mrs_lib::BatchVisualizer::addNullTriangle()462
mrs_lib::BatchVisualizer::addRay(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCone(mrs_lib::geometry::Cone const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addPath(mrs_msgs::Path_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::publish()462
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)79
mrs_lib::BatchVisualizer::addCuboid(mrs_lib::geometry::Cuboid const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::BatchVisualizer(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)214
mrs_lib::BatchVisualizer::BatchVisualizer()214
mrs_lib::BatchVisualizer::~BatchVisualizer()428
+
+
+ + + +
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..29453ad03f --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html @@ -0,0 +1,440 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-06-08 22:13:12Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::triangleArea(double, double, double)0
mrs_lib::geometry::quaternionBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)0
mrs_lib::geometry::quaternionFromEuler(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::geometry::quaternionFromEuler(double, double, double)0
mrs_lib::geometry::sphericalTriangleArea(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::haversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::rotationBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::angleaxisBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::cross(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>)3
mrs_lib::geometry::quaternionFromHeading(double)146787
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)153883
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)227972
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)373280
+
+
+ + + +
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..7e3a7bde4a --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-06-08 22:13:12Functions:81650.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)153883
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)146787
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&)373280
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)227972
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..99758dd62f --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.html @@ -0,0 +1,269 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-06-08 22:13:12Functions:81650.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : #include <mrs_lib/geometry/misc.h>
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             :   namespace geometry
+       7             :   {
+       8             : 
+       9             :     // instantiation of common template values
+      10             :     vec_t<3 + 1> toHomogenous(const vec_t<3>& vec);
+      11             :     vec_t<2 + 1> toHomogenous(const vec_t<2>& vec);
+      12             : 
+      13             :     // | ----------------- Angle-related functions ---------------- |
+      14             : 
+      15             :     /* angle-related functions //{ */
+      16             : 
+      17             :     /* cross() //{ */
+      18             : 
+      19           3 :     double cross(const vec2_t& vec1, const vec2_t vec2)
+      20             :     {
+      21           3 :       return vec1.x() * vec2.y() - vec1.y() * vec2.x();
+      22             :     }
+      23             : 
+      24             :     //}
+      25             : 
+      26             :     /* angleBetween() //{ */
+      27             : 
+      28      153883 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30      153883 :       const double sin_12 = vec1.cross(vec2).norm();
+      31      153868 :       const double cos_12 = vec1.dot(vec2);
+      32      153881 :       const double angle = std::atan2(sin_12, cos_12);
+      33      153881 :       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      146787 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94      293574 :       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      373280 :     double dist(const vec2_t& a, const vec2_t& b)
+     171             :     {
+     172             : 
+     173      373280 :       return (a - b).norm();
+     174             :     }
+     175             : 
+     176      227972 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179      227972 :       return (a - b).norm();
+     180             :     }
+     181             : 
+     182             :     //}
+     183             : 
+     184             :   }  // namespace geometry
+     185             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.overview.html b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html new file mode 100644 index 0000000000..bdcd7dc00e --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html @@ -0,0 +1,67 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/src/math/math.cpp.gcov.png b/mrs_lib/src/math/math.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..11021a1f7a3fe1b16c9ed60780115ac61956a437 GIT binary patch literal 388 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1?!VDz0Y_(eeq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?X?_wfUqO7#M{-T^vI^I^TxHN;NC+EH*!&eSr7D zzjEnZM%SO5_wU^Pv> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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..70adaea9d9 --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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..001ca1d8da --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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..1954cea1cf --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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..10d0f560f9 --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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..0f1a287453 --- /dev/null +++ b/mrs_lib/src/median_filter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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..f8eb7944b7 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions:161794.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::setMaxValue(double)2
mrs_lib::MedianFilter::setMinValue(double)2
mrs_lib::MedianFilter::setBufferLength(unsigned long)2
mrs_lib::MedianFilter::setMaxDifference(double)2
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)91
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)92
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)94
mrs_lib::MedianFilter::check(double)144989
mrs_lib::MedianFilter::median() const144995
mrs_lib::MedianFilter::full() const153885
mrs_lib::MedianFilter::add(double)153907
+
+
+ + + +
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..ebaba4a816 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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)153907
mrs_lib::MedianFilter::check(double)144989
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)91
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)94
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)92
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const153885
mrs_lib::MedianFilter::median() const144995
+
+
+ + + +
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..23b559564a --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.html @@ -0,0 +1,286 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-06-08 22:13:12Functions: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          94 :   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          94 :       m_max_diff(max_diff)
+      12             :   {
+      13          94 :     m_buffer.set_capacity(buffer_length);
+      14          94 :     m_buffer_sorted.reserve(buffer_length);
+      15          94 :   }
+      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          91 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33          91 :     *this = other;
+      34          91 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39          92 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41          92 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43          92 :     m_buffer = other.m_buffer;
+      44          92 :     m_buffer_sorted = other.m_buffer_sorted;
+      45          92 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48          92 :     m_min_valid = other.m_min_valid;
+      49          92 :     m_max_valid = other.m_max_valid;
+      50          92 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52         184 :     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      153907 :   void MedianFilter::add(const double value)
+      74             :   {
+      75      307786 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77      153899 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79      153889 :     m_median = std::nullopt;
+      80      153889 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84      144989 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86      144989 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88      144982 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89      289961 :     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      153885 :   bool MedianFilter::full() const
+     113             :   {
+     114      307776 :     std::scoped_lock lck(m_mtx);
+     115      307775 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120      144995 :   double MedianFilter::median() const
+     121             :   {
+     122      289989 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124      144993 :     if (m_median.has_value())
+     125          18 :       return m_median.value();
+     126             :   
+     127             :     // check if there are even any numbers to calculate the median from
+     128      144970 :     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      144962 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137      144976 :     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      144980 :     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      144959 :     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      144956 :     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      144988 :     if (even_set)
+     148      144972 :       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      144981 :     return m_median.value();
+     154             :   }
+     155             :   //}
+     156             : 
+     157             :   /* initialized() method //{ */
+     158           1 :   bool MedianFilter::initialized() const
+     159             :   {
+     160           1 :     std::scoped_lock lck(m_mtx);
+     161           2 :     return m_buffer.size() > 0;
+     162             :   }
+     163             :   //}
+     164             : 
+     165             :   /* setBufferLength() method //{ */
+     166           2 :   void MedianFilter::setBufferLength(const size_t buffer_length)
+     167             :   {
+     168           4 :     std::scoped_lock lck(m_mtx);
+     169             :     // the median may change if the some values are discarded
+     170           2 :     if (buffer_length < m_buffer.size())
+     171           0 :       m_median = std::nullopt;
+     172             :   
+     173           2 :     m_buffer.set_capacity(buffer_length);
+     174           2 :     m_buffer_sorted.reserve(buffer_length);
+     175           2 :   }
+     176             :   //}
+     177             : 
+     178             :   /* setMinValue() method //{ */
+     179           2 :   void MedianFilter::setMinValue(const double min_value)
+     180             :   {
+     181           2 :     std::scoped_lock lck(m_mtx);
+     182           2 :     m_min_valid = min_value;
+     183           2 :   }
+     184             :   //}
+     185             : 
+     186             :   /* setMaxValue() method //{ */
+     187           2 :   void MedianFilter::setMaxValue(const double max_value)
+     188             :   {
+     189           2 :     std::scoped_lock lck(m_mtx);
+     190           2 :     m_max_valid = max_value;
+     191           2 :   }
+     192             :   //}
+     193             : 
+     194             :   /* setMaxDifference() method //{ */
+     195           2 :   void MedianFilter::setMaxDifference(const double max_diff)
+     196             :   {
+     197           2 :     std::scoped_lock lck(m_mtx);
+     198           2 :     m_max_diff = max_diff;
+     199           2 :   }
+     200             :   //}
+     201             : 
+     202             : } // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html b/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html new file mode 100644 index 0000000000..f9295bd144 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html @@ -0,0 +1,71 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html new file mode 100644 index 0000000000..08d41606fa --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3710136.6 %
Date:2024-06-08 22:13:12Functions: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)1499
mrs_lib::Profiler::Profiler()1499
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1499
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)735864
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)736000
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)2039877
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)2039913
mrs_lib::Routine::end()2775721
mrs_lib::Routine::~Routine()2775789
+
+
+ + + +
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..b13f695c4b --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3710136.6 %
Date:2024-06-08 22:13:12Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Routine::end()2775721
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)2039877
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)736000
mrs_lib::Routine::~Routine()2775789
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)2039913
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)735864
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)1499
mrs_lib::Profiler::Profiler()1499
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1499
+
+
+ + + +
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..8e9889fbf3 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.html @@ -0,0 +1,301 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3710136.6 %
Date:2024-06-08 22:13:12Functions: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        1499 : Profiler::Profiler() {
+      11        1499 : }
+      12             : 
+      13        1499 : Profiler::Profiler(ros::NodeHandle& nh, std::string _node_name_, bool profiler_enabled) {
+      14             : 
+      15        1499 :   this->nh_                = std::make_shared<ros::NodeHandle>(nh);
+      16        1499 :   this->_node_name_        = _node_name_;
+      17        1499 :   this->_profiler_enabled_ = profiler_enabled;
+      18             : 
+      19        1499 :   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        1499 :   ROS_INFO("[%s]: profiler initialized", _node_name_.c_str());
+      25             : 
+      26        1499 :   this->is_initialized_ = true;
+      27        1499 : }
+      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        1499 : Profiler& Profiler::operator=(const Profiler& other) {
+      43             : 
+      44        1499 :   if (this == &other) {
+      45           0 :     return *this;
+      46             :   }
+      47             : 
+      48        1499 :   this->is_initialized_    = other.is_initialized_;
+      49        1499 :   this->nh_                = other.nh_;
+      50        1499 :   this->_node_name_        = other._node_name_;
+      51        1499 :   this->_profiler_enabled_ = other._profiler_enabled_;
+      52             : 
+      53        1499 :   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        1499 :   return *this;
+      59             : }
+      60             : 
+      61             : //}
+      62             : 
+      63             : /* Profiler::registerRoutine() for periodic //{ */
+      64             : 
+      65      735864 : Routine Profiler::createRoutine(std::string name, double expected_rate, double threshold, ros::TimerEvent event) {
+      66             : 
+      67      735864 :   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     2039913 : Routine Profiler::createRoutine(std::string name) {
+      75             : 
+      76     2039913 :   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      736000 : Routine::Routine(std::string name, std::string node_name, double expected_rate, double threshold, std::shared_ptr<ros::Publisher> publisher,
+      86      736000 :                  std::shared_ptr<std::mutex> mutex_publisher, bool profiler_enabled, ros::TimerEvent event) {
+      87             : 
+      88      734750 :   if (!profiler_enabled) {
+      89      734802 :     return;
+      90             :   }
+      91             : 
+      92           0 :   _threshold_ = threshold;
+      93             : 
+      94           0 :   this->publisher_       = publisher;
+      95           0 :   this->mutex_publisher_ = mutex_publisher;
+      96             : 
+      97           0 :   this->_routine_name_  = name;
+      98           0 :   msg_out_.routine_name = name;
+      99             : 
+     100           0 :   this->_node_name_  = node_name;
+     101           0 :   msg_out_.node_name = node_name;
+     102             : 
+     103           0 :   this->_profiler_enabled_ = profiler_enabled;
+     104             : 
+     105           0 :   msg_out_.is_periodic   = true;
+     106           0 :   msg_out_.expected_rate = expected_rate;
+     107             : 
+     108           0 :   msg_out_.expected_start = event.current_expected.toSec();
+     109           0 :   msg_out_.real_start     = event.current_real.toSec();
+     110             : 
+     111           0 :   msg_out_.stamp     = ros::Time::now();
+     112           0 :   msg_out_.duration  = 0;
+     113           0 :   msg_out_.iteration = this->iteration_++;
+     114           0 :   msg_out_.event     = mrs_msgs::ProfilerUpdate::START;
+     115             : 
+     116           0 :   execution_start_ = ros::Time::now();
+     117             : 
+     118           0 :   double dt = msg_out_.real_start - msg_out_.expected_start;
+     119             : 
+     120           0 :   if (dt > _threshold_) {
+     121           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: routine '%s' was lauched late by %.3f s!", _node_name_.c_str(), _routine_name_.c_str(), dt);
+     122             :   }
+     123             : 
+     124             :   {
+     125           0 :     std::scoped_lock lock(*mutex_publisher_);
+     126             : 
+     127             :     try {
+     128           0 :       publisher_->publish(mrs_msgs::ProfilerUpdateConstPtr(new mrs_msgs::ProfilerUpdate(msg_out_)));
+     129             :     }
+     130           0 :     catch (...) {
+     131           0 :       ROS_ERROR("Exception caught during publishing topic %s.", publisher_->getTopic().c_str());
+     132             :     }
+     133             :   }
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* Routine constructor for normal //{ */
+     139             : 
+     140     2039877 : Routine::Routine(std::string name, std::string node_name, std::shared_ptr<ros::Publisher> publisher, std::shared_ptr<std::mutex> mutex_publisher,
+     141     2039877 :                  bool profiler_enabled) {
+     142             : 
+     143     2038719 :   if (!profiler_enabled) {
+     144     2038737 :     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     2775721 : void Routine::end(void) {
+     186             : 
+     187     2775721 :   if (!_profiler_enabled_) {
+     188     2775382 :     return;
+     189             :   }
+     190             : 
+     191         348 :   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     2775519 : Routine::~Routine() {
+     213             : 
+     214     2775789 :   this->end();
+     215     2774013 : }
+     216             : 
+     217             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html b/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html new file mode 100644 index 0000000000..f66727ff8b --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html @@ -0,0 +1,75 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.png b/mrs_lib/src/profiler/profiler.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..44bb60b2c679000bfc9fc7b4f89cc2f0264fcdd8 GIT binary patch literal 827 zcmV-B1H}A^P)( zRA_jX4>?fp(O*C8=7vv$G0st@QIgJxr;(ykZ$z$lG3!tG4g=aZ^cF{d z^}aVvg#$Pj6yd${+5L}WQr79RxG5%sjugjnif}L}vb9PU9VvIAj9upw z=yEW`igkF0TIBsQuHj&-iL2DBh#KJ$n(C&&%Sr&4%-Q|F1Asl42_%@G-Pax#F_2~> z<`9<+ibY&b`3P|f)V+kb-X<<5F5+VlcM@zaNood zPWc5;lzsCWG)wD`)#9uGy-iU$EA;6JSnpif2l6(ol*G*zk?c_t7duTWM!GnGS`g|g zD^|O9Qq08lnPRnT+S}V$^d(fe2G3kT*Mf1FglDFa;(F8_saS(TePSov^v);S{NR;62%sgr)|yf^&jikY4nYD1YiIG{gVwg(KnwCXoPcj7rxDq(pec_y~%0UE`vn zQliCteU$ruK}vT0f>axsRe*Bg-SD|$`**`bKdAnZox literal 0 HcmV?d00001 diff --git a/mrs_lib/src/safety_zone/index-detail-sort-f.html b/mrs_lib/src/safety_zone/index-detail-sort-f.html new file mode 100644 index 0000000000..eb57393ff0 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-06-08 22:13:12Functions: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..0fa2448b84 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-06-08 22:13:12Functions: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..6b2251da27 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-06-08 22:13:12Functions: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..8b75862f29 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-06-08 22:13:12Functions: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..2542962fc6 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-06-08 22:13:12Functions: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..da305a13de --- /dev/null +++ b/mrs_lib/src/safety_zone/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-06-08 22:13:12Functions: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..9513e0d65f --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-06-08 22:13:12Functions: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>)626
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)696
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>)696
+
+
+ + + +
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..4aed8de6ad --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-06-08 22:13:12Functions: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>)626
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)696
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>)696
+
+
+ + + +
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..71d1ebed9e --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.html @@ -0,0 +1,143 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-06-08 22:13:12Functions: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         626 : 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         626 :   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         696 : Intersection::Intersection(bool intersect, bool parallel, Eigen::RowVector2d point) : point(std::move(point)), parallel(parallel), intersect(intersect){}
+      16             : 
+      17             : /* sectionIntersect() //{ */
+      18             : 
+      19         696 : Intersection sectionIntersect(Eigen::RowVector2d start1, Eigen::RowVector2d end1, Eigen::RowVector2d start2, Eigen::RowVector2d end2) {
+      20         696 :   Eigen::RowVector2d vector1 = end1 - start1;
+      21         696 :   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         696 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+      29         696 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector2(0);
+      30             : 
+      31         696 :   if (cross == 0) {
+      32             :     // are parallel
+      33          70 :     if (difference_cross != 0)
+      34          70 :       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         626 :   double             scale1 = difference_cross / cross;
+      48         626 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+      49         626 :   double             scale2 = getScale(start2, vector2, point);
+      50             : 
+      51         626 :   if (0 <= scale1 && scale1 <= 1 && 0 <= scale2 && scale2 <= 1) {
+      52           0 :     return Intersection{true, false, point};
+      53             :   }
+      54         626 :   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..45dce0b1c9 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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..3812212e0b --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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..539d8ae257 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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..74b969ac66 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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..fd93015557 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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..e0f5b1e5fd --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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..26ee86b6cd --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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>)85
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)174
mrs_lib::Polygon::isPointInside(double, double)10165
mrs_lib::Polygon::getPointMessageVector(double)24484
+
+
+ + + +
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..a659d8fdf3 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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)10165
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)174
mrs_lib::Polygon::getPointMessageVector(double)24484
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)85
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..283f7838b9 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html @@ -0,0 +1,290 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-06-08 22:13:12Functions: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          85 : Polygon::Polygon(const Eigen::MatrixXd vertices) : vertices(vertices) {
+      10             : 
+      11          85 :   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          85 :   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          85 :   Eigen::RowVector2d edge1;
+      21          85 :   Eigen::RowVector2d edge2 = vertices.row(1) - vertices.row(0);
+      22         425 :   for (int i = 0; i < vertices.rows(); ++i) {
+      23         340 :     edge1 = edge2;
+      24         340 :     edge2 = vertices.row((i + 2) % vertices.rows()) - vertices.row((i + 1) % vertices.rows());
+      25             : 
+      26         340 :     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         340 :     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          85 : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* isPointInside() //{ */
+      40             : 
+      41       10165 : bool Polygon::isPointInside(const double px, const double py) {
+      42             : 
+      43       10165 :   int count = 0;
+      44             : 
+      45             :   // Cast a horizontal ray and see how many times it intersects all the edges
+      46       50825 :   for (int i = 0; i < vertices.rows(); ++i) {
+      47       40660 :     double v1x = vertices(i, 0);
+      48       40660 :     double v1y = vertices(i, 1);
+      49       40660 :     double v2x = vertices((i + 1) % vertices.rows(), 0);
+      50       40660 :     double v2y = vertices((i + 1) % vertices.rows(), 1);
+      51             : 
+      52       40660 :     if (v1y > py && v2y > py)
+      53       10090 :       continue;
+      54       30570 :     if (v1y <= py && v2y <= py)
+      55       10390 :       continue;
+      56       20180 :     double intersect_x    = (v1y == v2y) ? v1x : (py - v1y) * (v2x - v1x) / (v2y - v1y) + v1x;
+      57       20180 :     bool   does_intersect = intersect_x > px;
+      58       20180 :     if (does_intersect)
+      59       10087 :       ++count;
+      60             :   }
+      61             : 
+      62       10165 :   return count % 2;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* doesSectionIntersect() //{ */
+      68             : 
+      69         174 : bool Polygon::doesSectionIntersect(const double startX, const double startY, const double endX, const double endY) {
+      70             : 
+      71         174 :   Eigen::RowVector2d start{startX, startY};
+      72         174 :   Eigen::RowVector2d end{endX, endY};
+      73             : 
+      74         870 :   for (int i = 0; i < vertices.rows(); ++i) {
+      75             : 
+      76         696 :     Eigen::RowVector2d edgeStart = vertices.row(i);
+      77         696 :     Eigen::RowVector2d edgeEnd   = vertices.row((i + 1) % vertices.rows());
+      78             : 
+      79         696 :     if (sectionIntersect(start, end, edgeStart, edgeEnd).intersect) {
+      80           0 :       return true;
+      81             :     }
+      82             :   }
+      83             : 
+      84         174 :   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       24484 : std::vector<geometry_msgs::Point> Polygon::getPointMessageVector(const double z) {
+     194       24484 :   std::vector<geometry_msgs::Point> points(vertices.rows());
+     195      122420 :   for (int i = 0; i < vertices.rows(); ++i) {
+     196       97936 :     points[i].x = vertices(i, 0);
+     197       97936 :     points[i].y = vertices(i, 1);
+     198       97936 :     points[i].z = z;
+     199             :   }
+     200             : 
+     201       24484 :   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..1402600481 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-06-08 22:13:12Functions: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&)85
mrs_lib::SafetyZone::~SafetyZone()85
mrs_lib::SafetyZone::isPathValid(double, double, double, double)174
mrs_lib::SafetyZone::isPointValid(double, double)10165
mrs_lib::SafetyZone::getBorder()12242
+
+
+ + + +
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..5627e5e1e9 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-06-08 22:13:12Functions: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)174
mrs_lib::SafetyZone::isPointValid(double, double)10165
mrs_lib::SafetyZone::getBorder()12242
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)85
mrs_lib::SafetyZone::~SafetyZone()85
+
+
+ + + +
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..437aa7b9d8 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-06-08 22:13:12Functions: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         170 : SafetyZone::~SafetyZone() {
+      17          85 :   delete outerBorder;
+      18          85 : }
+      19             : 
+      20             : //}
+      21             : 
+      22             : /* SafetyZone() //{ */
+      23             : 
+      24          85 : SafetyZone::SafetyZone(const Eigen::MatrixXd& outerBorderMatrix) {
+      25             : 
+      26             :   try {
+      27          85 :     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          85 : }
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* isPointValid() //{ */
+      43             : 
+      44       10165 : bool SafetyZone::isPointValid(const double px, const double py) {
+      45             : 
+      46       10165 :   if (!outerBorder->isPointInside(px, py)) {
+      47          78 :     return false;
+      48             :   }
+      49             : 
+      50       10087 :   return true;
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* isPathValid() //{ */
+      56             : 
+      57         174 : bool SafetyZone::isPathValid(const double p1x, const double p1y, const double p2x, const double p2y) {
+      58             : 
+      59         174 :   if (outerBorder->doesSectionIntersect(p1x, p1y, p2x, p2y)) {
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63         174 :   return true;
+      64             : }
+      65             : 
+      66             : //}
+      67             : 
+      68             : /* getBorder() //{ */
+      69             : 
+      70       12242 : Polygon SafetyZone::getBorder() {
+      71       12242 :   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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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 +
13.4%13.4%
+ 		13.4 %19 / 14250.0 %5 / 10
<unnamed>13.4 %19 / 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..1756594975 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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 +
13.4%13.4%
+ 		13.4 %19 / 14250.0 %5 / 10
<unnamed>13.4 %19 / 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..ac23dc0745 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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 +
13.4%13.4%
+ 		13.4 %19 / 14250.0 %5 / 10
<unnamed>13.4 %19 / 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..62bd2ab569 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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 +
13.4%13.4%
+ 		13.4 %19 / 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..771a5d6220 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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 +
13.4%13.4%
+ 		13.4 %19 / 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..d8f42d1164 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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 +
13.4%13.4%
+ 		13.4 %19 / 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..98bd6be107 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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)749
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()749
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4402600
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)5629690
mrs_lib::ScopeTimer::~ScopeTimer()5631614
+
+
+ + + +
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..8e13324c81 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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&)4402600
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)5629690
mrs_lib::ScopeTimer::~ScopeTimer()5631614
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)749
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()749
+
+
+ + + +
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..3030960729 --- /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:1914213.4 %
Date:2024-06-08 22:13:12Functions: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         749 : ScopeTimerLogger::ScopeTimerLogger(const std::string& logfile, const bool enable_logging, const int log_precision)
+      10         749 :     : _logging_enabled_(enable_logging), _log_filepath_(logfile) {
+      11             : 
+      12         749 :   if (!_logging_enabled_) {
+      13         749 :     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         749 : ScopeTimerLogger::~ScopeTimerLogger() {
+      47         749 :   if (_logging_enabled_) {
+      48           0 :     ROS_DEBUG("[%s]: ScopeTimerLogger: closing logstream.", ros::this_node::getName().c_str());
+      49           0 :     _logstream_.close();
+      50             :   }
+      51         749 : }
+      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     5629690 : ScopeTimer::ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable)
+     104     5629690 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(ros::Duration(0.0)), _logger_(scope_timer_logger) {
+     105             : 
+     106     5617297 :   checkpoints.push_back(time_point("timer start"));
+     107             : 
+     108     5615920 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     109     5615930 : }
+     110             : 
+     111             : //}
+     112             : 
+     113             : /* ScopeTimer::checkpoint() //{ */
+     114             : 
+     115     4402600 : void ScopeTimer::checkpoint(const std::string& label) {
+     116             : 
+     117     4402600 :   if (_enable_print_or_log) {
+     118           0 :     checkpoints.push_back(time_point(label));
+     119             :   }
+     120     4402600 : }
+     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    16889314 : ScopeTimer::~ScopeTimer() {
+     136             : 
+     137     5631614 :   if (!_enable_print_or_log) {
+     138     5631713 :     return;
+     139             :   }
+     140             : 
+     141           0 :   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     5629062 : }
+     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..5f1a78fab6cadb9b46ec83f0469855c9d8127029 GIT binary patch literal 1272 zcmVG9*1a=3M91S1@(s*GYAIWIs=&X*E z)sv>(oSl7W;s_x4CEnuc5=SLmPpy%o$=DJY_yc$Xv^hQ`;M|TE&}M9VT(cf=?x0j~ zlTc3wp(qrg(3f|~abXmF8Q9V{Ya7q;?}^~DE1Z?XpDe2)XB^o(%M!CjWwPIUt6`n$ z+j&=|=2r`mGH#tILb?&bE`Z~7W;dZ>K|mR8i^m20MByWKJ6HNi)(g83kb3CrKoI<{ zF>AA%8FAhqBMPZGD*1J++w~|@_$XieasmLQpL83P-uggq1y&r5WG_h<7IqShZ~^2} z5-)>x3ds-}?rXPXGBsrA{2VwP3` zp{o+b(UHnGUS}wPZp}x%a}@E=m0L9Gp0taqeF&r;QIjF{msy`!`cPGfMI6s>)U!)K zwskFC2AUF@`~KUHq@CH&-rRNldJll2a6@0%`0n2>rJJMB$CVJlmx)@9bI&#uZyUQDgFp>r zRIBS~G7r24%(F1$M|9l)V(z*gYO3dyRSo1!q14m!HmRPTQ5MxB`AxjXMmc7nv2f?d zX9X;=@o|%53&`&@zsRxZ7EWhvb_y#;pIVsGYfgO&$<1*8(cdB!9Mvn7K(C5Fl~L)g zknrS%z0Ar8_bs3no~Ba*?J}0eKI_{+!0YKGbe1@-%9#DIV3uAD{T}d%A70cBUiSi= z(ZRhlI{5l}Eva&F03o9!{8qoKbVdhX{Mk;+9m}NVjHV7&pqb;j0$i{R@u3epkk-Nf itKqw2(`x#o%6|aS^{4N~Ujy&}0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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..0e4c7bec35 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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..3525cf5c28 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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..49f717f5a5 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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..c5b5627d39 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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..a06a7aa3cc --- /dev/null +++ b/mrs_lib/src/timeout_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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..00cec66520 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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&)101
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)104
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)104
mrs_lib::TimeoutManager::started(unsigned long)1558
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)20463
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)199097
+
+
+ + + +
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..a869875da0 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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)104
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)20463
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)199097
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)104
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)1558
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&)101
+
+
+ + + +
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..839903718c --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html @@ -0,0 +1,193 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-06-08 22:13:12Functions: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         101 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7         101 :     : m_last_id(0)
+       8             :   {
+       9         101 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10         101 :   }
+      11             : 
+      12             : 
+      13         104 :   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         208 :     std::scoped_lock lck(m_mtx);
+      16         104 :     const auto new_id = m_timeouts.size();
+      17         104 :     m_timeouts.emplace_back(
+      18         104 :       timeout_info_t{
+      19         208 :         oneshot,
+      20             :         autostart,
+      21             :         callback,
+      22      199097 :         timeout,
+      23             :         last_reset,
+      24      199097 :         last_reset
+      25      199097 :       });
+      26      199097 :     return new_id;
+      27             :   }
+      28           9 : 
+      29             :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      30           9 :   {
+      31           9 :     std::scoped_lock lck(m_mtx);
+      32           9 :     m_timeouts.at(id).last_reset = time;
+      33             :   }
+      34         104 : 
+      35             :   void TimeoutManager::pause(const timeout_id_t id)
+      36         104 :   {
+      37         104 :     std::scoped_lock lck(m_mtx);
+      38         104 :     m_timeouts.at(id).started = false;
+      39         104 :   }
+      40             : 
+      41           2 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      42             :   {
+      43           4 :     std::scoped_lock lck(m_mtx);
+      44          10 :     m_timeouts.at(id).started = true;
+      45           8 :     m_timeouts.at(id).last_reset = time;
+      46           2 :   }
+      47             : 
+      48           4 :   void TimeoutManager::pauseAll()
+      49             :   {
+      50           8 :     std::scoped_lock lck(m_mtx);
+      51          20 :     for (auto& timeout_info : m_timeouts)
+      52             :       timeout_info.started = false;
+      53          16 :   }
+      54          16 : 
+      55             :   void TimeoutManager::startAll(const ros::Time& time)
+      56           4 :   {
+      57             :     std::scoped_lock lck(m_mtx);
+      58           0 :     for (auto& timeout_info : m_timeouts)
+      59             :     {
+      60           0 :       timeout_info.started = true;
+      61           0 :       timeout_info.last_reset = time;
+      62           0 :     }
+      63           0 :   }
+      64           0 : 
+      65           0 :   void TimeoutManager::change(const timeout_id_t id, const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset, const bool oneshot, const bool autostart)
+      66           0 :   {
+      67           0 :     std::scoped_lock lck(m_mtx);
+      68             :     auto& timeout_info = m_timeouts.at(id);
+      69           0 :     timeout_info.oneshot = oneshot;
+      70             :     timeout_info.started = autostart;
+      71           0 :     timeout_info.timeout = timeout;
+      72           0 :     timeout_info.callback = callback;
+      73             :     timeout_info.last_reset = last_reset;
+      74             :   }
+      75        1558 : 
+      76             :   ros::Time TimeoutManager::lastReset(const timeout_id_t id)
+      77        1558 :   {
+      78        3116 :     std::scoped_lock lck(m_mtx);
+      79             :     return m_timeouts.at(id).last_reset;
+      80             :   }
+      81       20463 : 
+      82             :   bool TimeoutManager::started(const timeout_id_t id)
+      83       20463 :   {
+      84             :     std::scoped_lock lck(m_mtx);
+      85       40926 :     return m_timeouts.at(id).started;
+      86       47208 :   }
+      87             : 
+      88             :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      89       26745 :   {
+      90       26745 :     const auto now = ros::Time::now();
+      91       26745 : 
+      92       26745 :     std::scoped_lock lck(m_mtx);
+      93             :     for (auto& timeout_info : m_timeouts)
+      94        2065 :     {
+      95        2065 :       // don't worry, these variables will get optimized out by the compiler
+      96             :       const bool started = timeout_info.started;
+      97        2065 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      98           0 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      99             :       if (started && last_reset_timeout && last_callback_timeout)
+     100             :       {
+     101       20463 :         timeout_info.callback(timeout_info.last_reset);
+     102             :         timeout_info.last_callback = now;
+     103             :         // if the timeout is oneshot, pause it
+     104             :         if (timeout_info.oneshot)
+     105             :           timeout_info.started = false;
+     106             :       }
+     107             :     }
+     108             :   }
+     109             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..1e6f9167ba --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html @@ -0,0 +1,48 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..bf94e45bc6c58a57d9c491eeff7fdeaf8b0b231f GIT binary patch literal 552 zcmV+@0@wYCP)M|&3AR^FnYt7O}>Cq?z-VM*iHa`IUUWeaSfq*E0 z$XC}`pB@*UFtsfk2`O;Kt84+C%#sZ6r)~sZZll9VW|0D}zgr4YV4j2eTB=JYbw@s) zm^BZ6)$9Azx4gzmECB!STsyCMiocW;C4y(d1IK(okv>^*eQXM*ypcVpoViCW49~in z-59eOC;Lv9^(q#~4YR=2MA@uj5fHg~h7M{O4ylE}5SnPF_sb1$$50P-j19h5fq^aB zz_1K?ybEDufaQSS&`8N6&{$0~@v*t5xI#VU^<|6W(c+4Sk3?ADBVK33{674O*QE{V z`@KTm?WHnNmBxcM*1_yLM5g|upv%1=IgVqSsPbvEEm=5eC`0Wjk* qw|M$ABkKkqxN)ptrEA4+;eG&o&5zS!cYcci0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-06-08 22:13:12Functions:151883.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ROSTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::threadFcn()8
mrs_lib::ThreadTimer::Impl::Impl(std::function<void (ros::TimerEvent const&)> const&, ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::~Impl()8
mrs_lib::ThreadTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ROSTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::stop()24
mrs_lib::ThreadTimer::Impl::start()24
mrs_lib::ThreadTimer::stop()24
mrs_lib::ThreadTimer::start()24
mrs_lib::ROSTimer::stop()24
mrs_lib::ROSTimer::start()24
mrs_lib::ThreadTimer::Impl::breakableSleep(ros::Time const&)208
mrs_lib::ThreadTimer::running()208
mrs_lib::ROSTimer::running()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..a3a57747ce --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-06-08 22:13:12Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ThreadTimer::Impl::breakableSleep(ros::Time const&)208
mrs_lib::ThreadTimer::Impl::stop()24
mrs_lib::ThreadTimer::Impl::start()24
mrs_lib::ThreadTimer::Impl::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::threadFcn()8
mrs_lib::ThreadTimer::Impl::Impl(std::function<void (ros::TimerEvent const&)> const&, ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::~Impl()8
mrs_lib::ThreadTimer::stop()24
mrs_lib::ThreadTimer::start()24
mrs_lib::ThreadTimer::running()208
mrs_lib::ThreadTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ROSTimer::setCallback(std::function<void (ros::TimerEvent const&)> const&)0
mrs_lib::ROSTimer::stop()24
mrs_lib::ROSTimer::start()24
mrs_lib::ROSTimer::running()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..58220475db --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10512584.0 %
Date:2024-06-08 22:13:12Functions:151883.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/src/timer/timer.cpp.gcov.png b/mrs_lib/src/timer/timer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..02902cc75b73433a383fcc0da608c79be7330961 GIT binary patch literal 1150 zcmV-^1cCdBP)gy$&H153T1yMq zl!ZVcu46InK!{6l+OOygpe-Dsfm)On2c3)p8daVQH3`@dwNkuja8`cWXQUjt4@aK?&(Bdv&G0bwx@znnfS<x<$a|&p_j@;g_@!{2j+`%AL1AQn`9OkB~5w4Ho_Tgxloj-;0aOMcC=(<(EtIn1T^QmHcu!A5q$!L!{D)pa&iGDK zZAz}#x2;I3UEmt85T$Fl&FEfga`)=Fy?fY=EOF#5th(7Tqd7KUevXla;MOhJMv<{} zWS$l+v=zVEWT1V>I!#ZEqYYPQPUs47pJc_WE~^&WN+A$H&eZ91%09rGhj>cSJRy2?H6x<-(J=Tf%vbN3vvEZ0 zf#GQXJh>f10<`a+CnwqhMYY*^MoLR$iM{Vnrm6i&WOjHJ1FcCF8}OOLsP@%?P9o#0 zj)Te|k=Mxs`GpWr7Cj`8UqS*@NJy$Dn!AklbTlM!t^b7EfG?;<&F>fg1GM;S?E+(5 Q?EnA(07*qoM6N<$f;*}mH2?qr literal 0 HcmV?d00001 diff --git a/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html b/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html new file mode 100644 index 0000000000..08971086b2 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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..b6e13ab651 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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..f4db809d1a --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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..a04c369476 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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..d7592f4f12 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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..448f55a04d --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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..4cbce733ab --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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()205
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1990062
+
+
+ + + +
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..ce793883df --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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&)1990062
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()205
+
+
+ + + +
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..0a5bbcb90f --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-06-08 22:13:12Functions: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         205 : TransformBroadcaster::TransformBroadcaster() {
+       8         205 :   broadcaster_ = tf2_ros::TransformBroadcaster();
+       9         205 : }
+      10             : //}
+      11             : 
+      12             : /* sendTransform (const geometry_msgs::TransformStamped &transform) //{ */
+      13     1990062 : void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) {
+      14     3978356 :   std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      15     1986750 :   if (last_messages_.count(frames_from_to) > 0) {
+      16     1988142 :     if (transform.header.stamp > last_messages_[frames_from_to]) {
+      17     1676783 :       broadcaster_.sendTransform(transform);
+      18     1679404 :       last_messages_[frames_from_to] = transform.header.stamp;
+      19             :     } else {
+      20      307935 :       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        2135 :     std::pair<std::string, ros::Time> new_pair;
+      25        1047 :     new_pair.first  = frames_from_to;
+      26        1047 :     new_pair.second = transform.header.stamp;
+      27        1047 :     broadcaster_.sendTransform(transform);
+      28        1047 :     last_messages_.insert(new_pair);
+      29             :   }
+      30     1990300 : }
+      31             : //}
+      32             : 
+      33             : /* sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) //{ */
+      34           0 : void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) {
+      35           0 :   for (auto transform : transforms) {
+      36           0 :     std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      37           0 :     if (last_messages_.count(frames_from_to) > 0) {
+      38           0 :       if (transform.header.stamp > last_messages_[frames_from_to]) {
+      39           0 :         broadcaster_.sendTransform(transform);
+      40           0 :         last_messages_[frames_from_to] = transform.header.stamp;
+      41             :       } else {
+      42           0 :         ROS_WARN_ONCE("[%s]: TF_REPEATED_DATA ignoring data with redundant timestamp. Transform from frame '%s' to frame '%s'",
+      43             :                       ros::this_node::getName().c_str(), transform.header.frame_id.c_str(), transform.child_frame_id.c_str());
+      44             :       }
+      45             :     } else {
+      46           0 :       std::pair<std::string, ros::Time> new_pair;
+      47           0 :       new_pair.first  = frames_from_to;
+      48           0 :       new_pair.second = transform.header.stamp;
+      49           0 :       broadcaster_.sendTransform(transform);
+      50           0 :       last_messages_.insert(new_pair);
+      51             :     }
+      52             :   }
+      53           0 : }
+      54             : //}
+      55             : 
+      56             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html new file mode 100644 index 0000000000..f1fb284078 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html @@ -0,0 +1,34 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ef243e6e5fb1d3bcaec6f7b6354ea8ce8ebffdea GIT binary patch literal 425 zcmV;a0apHrP)M01uJ*nY`JnFx*N2@5{2?`a1q6zmssfL3_>!}8X35VVlYkpKa402t<;> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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..8a4f3de0a6 --- /dev/null +++ b/mrs_lib/src/transformer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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..c41e023d9b --- /dev/null +++ b/mrs_lib/src/transformer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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..7623f89f20 --- /dev/null +++ b/mrs_lib/src/transformer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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..252601e94b --- /dev/null +++ b/mrs_lib/src/transformer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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..84d982091a --- /dev/null +++ b/mrs_lib/src/transformer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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..62f9ec3af2 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.func-sort-c.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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&)744
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2120
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2120
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2122
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4250
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&)24027
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)146781
mrs_lib::Transformer::setLatLon(double, double)153595
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&)1774753
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7450473
+
+
+ + + +
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..b319bcc481 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.func.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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&)24027
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&)146781
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4250
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&)1774753
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7450473
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&)2120
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&)2120
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2122
mrs_lib::Transformer::setLatLon(double, double)153595
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)744
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..c0c1c08baf --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.gcov.html @@ -0,0 +1,671 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-06-08 22:13:12Functions: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         744 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49         744 :     : 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         744 :   }
+      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       24027 :   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       48064 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82       24037 :     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       48074 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90       48074 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91       48074 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93       24037 :     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      153595 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119      153595 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122      152875 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123      152961 :     got_utm_zone_ = true;
+     124      152455 :   }
+     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      146781 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255      293562 :     geometry_msgs::PoseStamped pose;
+     256      146781 :     pose.header = what.header;
+     257             :   
+     258      146781 :     pose.pose.position.x = what.reference.position.x;
+     259      146781 :     pose.pose.position.y = what.reference.position.y;
+     260      146781 :     pose.pose.position.z = what.reference.position.z;
+     261      146781 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264      293562 :     const auto pose_opt = transformImpl(tf, pose);
+     265      146781 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268      146781 :     pose = pose_opt.value();
+     269             :   
+     270      293562 :     mrs_msgs::ReferenceStamped ret;
+     271      146781 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274      146781 :     ret.reference.position.x = pose.pose.position.x;
+     275      146781 :     ret.reference.position.y = pose.pose.position.y;
+     276      146781 :     ret.reference.position.z = pose.pose.position.z;
+     277      146781 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278      146781 :     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     1774753 :   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     1774753 :     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     1774753 :     if (from_frame.empty() || to_frame.empty())
+     312             :     {
+     313        1039 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform to/from an empty frame!", node_name_.c_str());
+     314        1039 :       return std::nullopt;
+     315             :     }
+     316             : 
+     317             :     // if the frames are the same, just return an identity transform
+     318     1773704 :     if (from_frame == to_frame)
+     319      436295 :       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     1555570 :     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     1555563 :     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        4246 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337        4246 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338        2123 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341        2123 :       frame_to(*tf_opt) = to_frame;
+     342        2123 :       return tf_opt;
+     343             :     }
+     344             : 
+     345     4660285 :     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     2141659 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352     1930412 :     catch (tf2::TransformException& e)
+     353             :     {
+     354      965212 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358      965197 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362     1914070 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364       32468 :       catch (tf2::TransformException& e)
+     365             :       {
+     366       16234 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371       16264 :     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       16264 :       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       16264 :     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     7450473 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466     7450473 :     if (frame_id.empty())
+     467        7523 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470     7442959 :     if (prefix_.empty())
+     471     4567178 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474     2875667 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475     1812460 :       return prefix_ + frame_id;
+     476             : 
+     477     1063350 :     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        2122 :   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        2122 :     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        2121 :     geometry_msgs::Point latlon;
+     532        2121 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533        2121 :     latlon.z = what.z;
+     534        2121 :     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        2120 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552        2120 :     const auto opt = UTMtoLL(what.position, prefix);
+     553        2120 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556        2120 :     geometry_msgs::Pose ret;
+     557        2120 :     ret.position = opt.value();
+     558        2120 :     ret.orientation = what.orientation;
+     559        2120 :     return ret;
+     560             :   }
+     561             : 
+     562        2120 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564        2120 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565        2120 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568        4240 :     geometry_msgs::PoseStamped ret;
+     569        2120 :     ret.header.frame_id = prefix + "utm_origin";
+     570        2120 :     ret.header.stamp = what.header.stamp;
+     571        2120 :     ret.pose = opt.value();
+     572        2120 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577        4250 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579        4250 :     const auto firstof = frame_id.find_first_of('/');
+     580        4250 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583        4250 :       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..a02e326b13 --- /dev/null +++ b/mrs_lib/src/utils/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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..b9dbf243de --- /dev/null +++ b/mrs_lib/src/utils/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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..40990abf42 --- /dev/null +++ b/mrs_lib/src/utils/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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..d0d7f7e29c --- /dev/null +++ b/mrs_lib/src/utils/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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..b18079cdf1 --- /dev/null +++ b/mrs_lib/src/utils/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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..d2b0be9b88 --- /dev/null +++ b/mrs_lib/src/utils/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utilsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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..72deaa75a6 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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>&)573429
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()573441
+
+
+ + + +
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..10fd33a004 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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>&)573429
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()573441
+
+
+ + + +
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..e125f22063 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.html @@ -0,0 +1,101 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/utils - utils.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-06-08 22:13:12Functions: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      573429 : AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>& in)
+       7      573429 :   : variable(in)
+       8             : {
+       9      573429 :   variable = true;
+      10      573442 : }
+      11             : 
+      12     1146883 : AtomicScopeFlag::~AtomicScopeFlag()
+      13             : {
+      14      573441 :   variable = false;
+      15      573442 : }
+      16             : 
+      17             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/utils.cpp.gcov.overview.html b/mrs_lib/src/utils/utils.cpp.gcov.overview.html new file mode 100644 index 0000000000..f3b2e0a717 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.overview.html @@ -0,0 +1,25 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()2
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()19
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)24
(anonymous namespace)::ProxyExec0::ProxyExec0()31
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()31
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)47
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)63
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()78
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiCapabilities(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)608
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3108
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()9534
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()9534
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()9534
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()9688
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()9688
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()9688
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)18480
mrs_uav_autostart::automatic_start::Topic::getTime()38214
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)47649
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&) const47652
mrs_uav_autostart::automatic_start::Topic::updateTime()47655
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)119542
+
+
+ + + +
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..43a24f0030 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()31
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()9534
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)47
mrs_uav_autostart::automatic_start::AutomaticStart::genericCallback(boost::shared_ptr<topic_tools::ShapeShifter const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int)47649
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()9688
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()9534
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()9534
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)119542
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()9688
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()9688
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)24
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiCapabilities(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)608
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3108
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()2
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()31
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()19
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)18480
mrs_uav_autostart::automatic_start::Topic::updateTime()47655
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()78
mrs_uav_autostart::automatic_start::Topic::getTime()38214
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)63
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()::{lambda(boost::shared_ptr<topic_tools::ShapeShifter const> const&)#1}::operator()(boost::shared_ptr<topic_tools::ShapeShifter const> const&) const47652
+
+
+ + + +
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..705f614d05 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,1077 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.png b/mrs_uav_autostart/src/automatic_start.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6b89348af3becb2aa858feaabc8a52a9df0faa6b GIT binary patch literal 3175 zcmV-t44CtYP)cE0Ro5uT zs6d1~MEl{Ewtwp*F0J>lmfb=s1ptn{T+6PJ@1lUrb<(I1k}!-Jja9%aX!J59iL^8- zin?AJ*=q-iHgaOnND{%PQ7+Z;>jFo*24^pn9D0mxK;tGD82L=Ioe4XxaREaxVrl(f4I_ff0bb;b^hDI(OmikprLY)E0h8h-NjPBbkwE!J0C%X?Lso-zDf*fuI ze>`vf7$LT7Z4JPB+FW3)-2p0FceCLzLh(4X;5#$o!Pc^@;j21~S#3ZE+X4(MGyo&3 zYxqQLeJ&P_JgyxL5pYI@8$>Ci!a2vD)40jd$O!?4;x+6Xxyk=oq9~anK*IIwwvsSC z#&smNR89C?`tYg~4>|?^eu8C#DECf?qg=-<+{NoH;cZ5aeqp{G9@oN&T2b9bBR>HxOfEQU0*j+jUr z|1B;W$wDFL_uK!WWQ zBi#mAQcpfBNKB3(T#q{de-)<0lLi)6i;B<8>i(L&O0oJ1Lo{wt=5w)dNs^;#WVq$& zeH~+-vk7wT^&-o4{dg5z*Vrz(o^GcUTu(Bcb6xi-IoAZnoa=t7GK_(1=S)PCVKR_F z>Y-mlFak92!?2;J;v+Ja)WRepZLAo%?0R$GSqt&x{xMmb^Oqk}50@R2hl`8dW#gKG%m>%Y3)@kv^6_0~#nS8CtSju_oJ^DQAo^F1U-ReX^)7P~M;b=Mdg z)%B6#KL8*fW3+Ik>n847l`Xryt__H;ws@U?=7}<&{2nYzFXjIK)?*2MpdA}-0pI} zKa=%!{{DJCUcc&|$2H;RBLYrjj}UG8e#D$xPjXG%^8?=4cVmO+gxMZb)q%c5DI0U3h?e zjG6>wYlaIb-%j<+R82zlG9y3KkOX3iWy%J?z%0>#8P9sEZ%fWG zU_IE%0D0HpYevrVp628zM_TVGY$JR%5f!#DYA6u78nf3-8lk3D&MX9E7Ct(*zNkh@ ztSthj|L&F*qX8fj;{c6PPfMUN$mO9^#|^#K1~ZO&DktpQdO61lLkE71Ga7ctFmCOY zO=`M7RoJOJI0S@24hQcD?EH0#WR2;3O+P0S%4SSTCSf;LVMaae=5?b8$B?v%Zq2m% zJ#WvX;*bIb=cHrw(yDwgnQ><(-u1%qb&cIZGg8NO-(fed4S2L3WV`gtc=c3{O*k|5 zKUANKx0Zo3(|j6`x%J&vd>@ZS$@c}wi*Xm6879-PSU{)j{Kn!1=l|+FTp;VJ=eSn5 zKGFS;QZ5j^`FS{deaGc&cAAAW3i{&n>x|+kTJjyvo~!G&c2tP>!ebt6NSUA6XKMYfbZ@>(+pN^(%tMKnD^^iMY_G0Peq^W z8o0b1kgu78b?NGJ&8ms7@0ue048$IZOeBZV80O`;fDq#Z*8rop>?0nXaCW*nkjXi$ zs;95L52TW|7IZwgofuWqcsgmU5NWF7+8w@LjKDirM`F4#8g;A6`5wgBDB{?Q__@*{ zExR61GpY43Upl~cqH9l>Xzt^rLkMO_JV)E#8$gb~?_-9s^A)Z0C3U1*e5OLck(N#p ztdX{KD894+53=|=9A|vZQb;_+G#+&JG1bHP844dNR&>kV$HOpkTNn`!R3X7NW>u4; z9104>st5&SV`Oy}x}8)W6y~_*1O*=znnA%uh1VBft*Ed&Zh*E48aa9Rxn!@{j>1^= zJ&Lak)k8+164w|%8Rd$kGbYvYV@MIOAwT~T~EqG}xA7WvyBz2|gTw@NP zPT|mTMaYBSrjM)t{Pp~bfQ>wEVJ3S$bM}jQP2I%n-txs`+B+#mDR*HqZMeRqMZ4L0 zdFTWpP&9^{@@s`w(aRDszGw2L%1n;3J36Op!cB?uO4yt4oTWc?7UN3rk|G zHIa9Ih+{9=!{vEVh=$>pbCFBA=^SWH?~XH#M}|E!(sKaGRe|1`w@##X@?yK;t^{&j zX|g4x$P|X)l=v{ls+PLMBYR{TT>IQi9T!~f_a9-nau=VP7>gl9Xsa!ZtgWR-w0?7A zvG6P3Fj6hoS*%SKsLb*8byIM&HBs(7`{vU~e@3dCa%RFdQ>O3i3;BODz~+Oy4v;&` z#a;42bI(fqHNhM4Bo8+~?49F$y!jOH;|WS6Pi$qvI00Gm=@Ad8ABhZe?yUMzpFMi< zJRZ1UqIh`L<1jX5d6mF_R~TjiUN98Pd}Y@KQ=j4FliYvMIG*4zP98KRn#i@Fne}uA zFxFqG(2ud`&0;RKb0nr!Qs~2&T;fFD^ySOpVl;|hx%?n`TKRjr2<*BPt_Ucde2DY1 z)OiVSO*s>c=LG|&cPFzZM&`~PTVHyg5%PW5T>(42V{R`zIWCA1WUy`xVZ_{Cj_?P$ zJ)vnFsAZ@kJ6Z@H^Z)VwF=a+4Z21m?a^p`&5p!{7#IrqdN7`iCRO6HzIBhfiiwDWC zep*^d*HZi+Q%mU^`;J(6fQ@H5r5x#)c>eN7iFK!PE|)*rlPVI|C?aEN-Y{55rNa}F04q?ZYl)aMaKAM?a zGF4`nH17URr)l{V^`0Xc304p=FR_JDvt*Ja1ti|!(c>TA_M#tVJ>!$wSCW}{d9Lxm z*_kTPhSQG`o$gxr) z@mHgVtCxI(QRENsL4il}PDPk}?6IEq6O{%~{;u%XzM_nG)a}vtnRi|Hk{{){g&%4; z@v}U!6b>De_z8}*%pDU659_*aJ#P$g*{coKZSoa59YOQW1&tyk*_G4C+}gdJra5ww zXU)~OF)y`g6IZQpPYs+e(YZi8sS3;6RuoUkh@%+(lb(4BRyq6%>-n${ci{vZrh*Hq z3O~f{{e`&P`%Hwm+D``hA-a|iapW@y*YvPIp@MjnWP}=yLE)=jheb{=b_#ce?>LzCq>p=hj N002ovPDHLkV1g?l{fqzr 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..a814a9f471 --- /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:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
<unnamed>87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail-sort-l.html b/mrs_uav_autostart/src/index-detail-sort-l.html new file mode 100644 index 0000000000..3a3bbdcad3 --- /dev/null +++ b/mrs_uav_autostart/src/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
<unnamed>87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail.html b/mrs_uav_autostart/src/index-detail.html new file mode 100644 index 0000000000..8ea28e4d98 --- /dev/null +++ b/mrs_uav_autostart/src/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
<unnamed>87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-f.html b/mrs_uav_autostart/src/index-sort-f.html new file mode 100644 index 0000000000..11e79cdaa4 --- /dev/null +++ b/mrs_uav_autostart/src/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-l.html b/mrs_uav_autostart/src/index-sort-l.html new file mode 100644 index 0000000000..2990d63886 --- /dev/null +++ b/mrs_uav_autostart/src/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index.html b/mrs_uav_autostart/src/index.html new file mode 100644 index 0000000000..1c22f4e5af --- /dev/null +++ b/mrs_uav_autostart/src/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:29333587.5 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.func-sort-c.html b/mrs_uav_controllers/include/common.h.func-sort-c.html new file mode 100644 index 0000000000..6f748c39b8 --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-06-08 22:13:12Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)4
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)9
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)40
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)51
+
+
+ + + +
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..5967811df8 --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-06-08 22:13:12Functions:99100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::setSaturation(double)14399
mrs_uav_controllers::PIDController::reset()1284
mrs_uav_controllers::PIDController::update(double const&, double const&)14399
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)1284
mrs_uav_controllers::PIDController::PIDController()1284
+
+
+ + + +
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..0ce463707e --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.gcov.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-06-08 22:13:12Functions: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        1284 : PIDController::PIDController() {
+      43             : 
+      44        1284 :   this->reset();
+      45        1284 : }
+      46             : 
+      47        1284 : void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) {
+      48             : 
+      49        1284 :   this->_kp_       = kp;
+      50        1284 :   this->_kd_       = kd;
+      51        1284 :   this->_ki_       = ki;
+      52        1284 :   this->saturation = saturation;
+      53        1284 :   this->antiwindup = antiwindup;
+      54        1284 : }
+      55             : 
+      56       14399 : void PIDController::setSaturation(const double saturation) {
+      57             : 
+      58       14399 :   this->saturation = saturation;
+      59       14399 : }
+      60             : 
+      61        1284 : void PIDController::reset(void) {
+      62             : 
+      63        1284 :   this->last_error_ = 0;
+      64        1284 :   this->integral_   = 0;
+      65        1284 : }
+      66             : 
+      67       14399 : double PIDController::update(const double &error, const double &dt) {
+      68             : 
+      69             :   // calculate the control error difference
+      70       14399 :   double difference = (error - last_error_) / dt;
+      71       14399 :   last_error_       = error;
+      72             : 
+      73       14399 :   double p_component = _kp_ * error;       // proportional feedback
+      74       14399 :   double d_component = _kd_ * difference;  // derivative feedback
+      75       14399 :   double i_component = _ki_ * integral_;   // derivative feedback
+      76             : 
+      77       14399 :   double sum = p_component + d_component + i_component;
+      78             : 
+      79       14399 :   if (saturation > 0) {
+      80       14399 :     if (sum >= saturation) {
+      81          37 :       sum = saturation;
+      82       14362 :     } else if (sum <= -saturation) {
+      83         119 :       sum = -saturation;
+      84             :     }
+      85             :   }
+      86             : 
+      87       14399 :   if (antiwindup > 0) {
+      88       14399 :     if (std::abs(sum) < antiwindup) {
+      89             :       // add to the integral
+      90       13888 :       integral_ += error * dt;
+      91             :     }
+      92             :   }
+      93             : 
+      94             :   // return the summ of the components
+      95       14399 :   return sum;
+      96             : }
+      97             : 
+      98             : }  // namespace mrs_uav_controllers
+      99             : 
+     100             : #endif  // PID_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.overview.html b/mrs_uav_controllers/include/pid.hpp.gcov.overview.html new file mode 100644 index 0000000000..1e2dbca381 --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.gcov.overview.html @@ -0,0 +1,45 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.png b/mrs_uav_controllers/include/pid.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..210752eb0f7abbe4f604ca452c63cabd54fcbebc GIT binary patch literal 519 zcmV+i0{H!jP)i;6ZDy3l1+EM97q1wFfy?;^t%ULDl)5$x zudZxs9|wgN#&S+{%-+D_U+^5KcAHUcKec%`Ff)4a1ZJuwkkpJ?V2Hm?hhj?cyI$-j z6IR|&6Z#l1hU=$#>Vf|txG3H9(th`RKN%W%c##Z|uGZuhz)xh|)X=9I-kXLt8S|8a zV%YX4lM~-WcfrHDO)|?9Z_{+J>{Z4~pHXJ@bSWS)bb#Sm1{SzJ&~<@(G5U+N_@Nr8 zXB3uUa|&Kps($zM!m literal 0 HcmV?d00001 diff --git a/mrs_uav_controllers/src/common.cpp.func-sort-c.html b/mrs_uav_controllers/src/common.cpp.func-sort-c.html new file mode 100644 index 0000000000..f47380f171 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-06-08 22:13:12Functions: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&)253
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3972
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&)7958
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)10693
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&)92666
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&)92666
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)94388
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&)94388
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)98907
+
+
+ + + +
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..f954066fe9 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-06-08 22:13:12Functions: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&)92666
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3972
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)98907
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)253
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)10693
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)94388
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&)94388
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&)92666
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&)7958
+
+
+ + + +
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..5de7b0a123 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.gcov.html @@ -0,0 +1,471 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-06-08 22:13:12Functions: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       94388 : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd) {
+      12             : 
+      13             :   // orientation error
+      14       94388 :   Eigen::Matrix3d R_error = 0.5 * (Rd.transpose() * R - R.transpose() * Rd);
+      15             : 
+      16             :   // vectorize the orientation error
+      17             :   // clang-format off
+      18       94388 :     Eigen::Vector3d R_error_vec;
+      19       94388 :     R_error_vec << (R_error(1, 2) - R_error(2, 1)) / 2.0,
+      20       94388 :                    (R_error(2, 0) - R_error(0, 2)) / 2.0,
+      21       94388 :                    (R_error(0, 1) - R_error(1, 0)) / 2.0;
+      22             :   // clang-format on
+      23             : 
+      24      188776 :   return R_error_vec;
+      25             : }
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* sanitizeDesiredForce() //{ */
+      30             : 
+      31       92666 : 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       92666 :   double theta = acos(input(2));
+      36       92666 :   double phi   = atan2(input(1), input(0));
+      37             : 
+      38             :   // check for the failsafe limit
+      39       92666 :   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       92666 :   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       92666 :   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       92666 :   Eigen::Vector3d output(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
+      63             : 
+      64       92666 :   return {output};
+      65             : }
+      66             : 
+      67             : //}
+      68             : 
+      69             : /* so3transform() //{ */
+      70             : 
+      71       92666 : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
+      72             : 
+      73       92666 :   Eigen::Vector3d body_z_normed = body_z.normalized();
+      74             : 
+      75       92666 :   Eigen::Matrix3d Rd;
+      76             : 
+      77       92666 :   if (preserve_heading) {
+      78             : 
+      79       24655 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Baca's method");
+      80             : 
+      81             :     // | ------------------------- body z ------------------------- |
+      82       24655 :     Rd.col(2) = body_z_normed;
+      83             : 
+      84             :     // | ------------------------- body x ------------------------- |
+      85             : 
+      86             :     // construct the oblique projection
+      87       24655 :     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       49310 :     Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+      91       24655 :     A.col(0)          = projector_body_z_compl.col(0);
+      92       24655 :     A.col(1)          = projector_body_z_compl.col(1);
+      93             : 
+      94             :     // create the basis of the projection null-space complement
+      95       49310 :     Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+      96       24655 :     B.col(0)          = Eigen::Vector3d(1, 0, 0);
+      97       24655 :     B.col(1)          = Eigen::Vector3d(0, 1, 0);
+      98             : 
+      99             :     // oblique projector to <range_basis>
+     100       49310 :     Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     101       49310 :     Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     102       24655 :     Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     103             : 
+     104       24655 :     Rd.col(0) = oblique_projector * heading;
+     105       24655 :     Rd.col(0).normalize();
+     106             : 
+     107             :     // | ------------------------- body y ------------------------- |
+     108             : 
+     109       24655 :     Rd.col(1) = Rd.col(2).cross(Rd.col(0));
+     110       24655 :     Rd.col(1).normalize();
+     111             : 
+     112             :   } else {
+     113             : 
+     114       68011 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Lee's method");
+     115             : 
+     116       68011 :     Rd.col(2) = body_z_normed;
+     117       68011 :     Rd.col(1) = Rd.col(2).cross(heading);
+     118       68011 :     Rd.col(1).normalize();
+     119       68011 :     Rd.col(0) = Rd.col(1).cross(Rd.col(2));
+     120       68011 :     Rd.col(0).normalize();
+     121             :   }
+     122             : 
+     123      185332 :   return Rd;
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* getLowestOutput() //{ */
+     129             : 
+     130       98907 : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     131             : 
+     132       98907 :   if (outputs.actuators) {
+     133        2580 :     return ACTUATORS_CMD;
+     134             :   }
+     135             : 
+     136       96327 :   if (outputs.control_group) {
+     137        2593 :     return CONTROL_GROUP;
+     138             :   }
+     139             : 
+     140       93734 :   if (outputs.attitude_rate) {
+     141       85037 :     return ATTITUDE_RATE;
+     142             :   }
+     143             : 
+     144        8697 :   if (outputs.attitude) {
+     145        2456 :     return ATTITUDE;
+     146             :   }
+     147             : 
+     148        6241 :   if (outputs.acceleration_hdg_rate) {
+     149        1162 :     return ACCELERATION_HDG_RATE;
+     150             :   }
+     151             : 
+     152        5079 :   if (outputs.acceleration_hdg) {
+     153         900 :     return ACCELERATION_HDG;
+     154             :   }
+     155             : 
+     156        4179 :   if (outputs.velocity_hdg_rate) {
+     157        2296 :     return VELOCITY_HDG_RATE;
+     158             :   }
+     159             : 
+     160        1883 :   if (outputs.velocity_hdg) {
+     161        1351 :     return VELOCITY_HDG;
+     162             :   }
+     163             : 
+     164         532 :   if (outputs.position) {
+     165         532 :     return POSITION;
+     166             :   }
+     167             : 
+     168           0 :   return {};
+     169             : }
+     170             : 
+     171             : //}
+     172             : 
+     173             : /* getHighestOutput() //{ */
+     174             : 
+     175       10693 : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     176             : 
+     177       10693 :   if (outputs.position) {
+     178           3 :     return POSITION;
+     179             :   }
+     180             : 
+     181       10690 :   if (outputs.velocity_hdg) {
+     182           5 :     return VELOCITY_HDG;
+     183             :   }
+     184             : 
+     185       10685 :   if (outputs.velocity_hdg_rate) {
+     186         490 :     return VELOCITY_HDG_RATE;
+     187             :   }
+     188             : 
+     189       10195 :   if (outputs.acceleration_hdg) {
+     190           5 :     return ACCELERATION_HDG;
+     191             :   }
+     192             : 
+     193       10190 :   if (outputs.acceleration_hdg_rate) {
+     194           6 :     return ACCELERATION_HDG_RATE;
+     195             :   }
+     196             : 
+     197       10184 :   if (outputs.attitude) {
+     198        5916 :     return ATTITUDE;
+     199             :   }
+     200             : 
+     201        4268 :   if (outputs.attitude_rate) {
+     202        1434 :     return ATTITUDE_RATE;
+     203             :   }
+     204             : 
+     205        2834 :   if (outputs.control_group) {
+     206        1417 :     return CONTROL_GROUP;
+     207             :   }
+     208             : 
+     209        1417 :   if (outputs.actuators) {
+     210        1417 :     return ACTUATORS_CMD;
+     211             :   }
+     212             : 
+     213           0 :   return {};
+     214             : }
+     215             : 
+     216             : //}
+     217             : 
+     218             : /* extractThrottle() //{ */
+     219             : 
+     220         253 : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output) {
+     221             : 
+     222         253 :   if (!control_output.control_output) {
+     223         137 :     return {};
+     224             :   }
+     225             : 
+     226         232 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* attitudeController() //{ */
+     232             : 
+     233       94388 : 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       94388 :   Eigen::Matrix3d R  = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     238       94388 :   Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
+     239             : 
+     240             :   // calculate the orientation error
+     241       94388 :   Eigen::Vector3d E = common::orientationError(R, Rd);
+     242             : 
+     243       94388 :   Eigen::Vector3d rate_feedback = gains.array() * E.array() + ff_rate.array();
+     244             : 
+     245             :   // | ----------- parasitic heading rate compensation ---------- |
+     246             : 
+     247       94388 :   if (parasitic_heading_rate_compensation) {
+     248             : 
+     249       23394 :     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       23394 :     Eigen::Vector3d rp_heading_rate_compensation(0, 0, 0);
+     253             : 
+     254       23394 :     Eigen::Vector3d q_feedback_yawless = rate_feedback;
+     255       23394 :     q_feedback_yawless(2)              = 0;  // nullyfy the effect of the original yaw feedback
+     256             : 
+     257       23394 :     double parasitic_heading_rate = 0;
+     258             : 
+     259             :     try {
+     260       23394 :       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       23394 :       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       23394 :     rate_feedback += rp_heading_rate_compensation;
+     274             :   }
+     275             : 
+     276             :   // | --------------- saturate the attitude rate --------------- |
+     277             : 
+     278       94388 :   if (rate_feedback(0) > rate_saturation(0)) {
+     279           0 :     rate_feedback(0) = rate_saturation(0);
+     280       94388 :   } else if (rate_feedback(0) < -rate_saturation(0)) {
+     281           0 :     rate_feedback(0) = -rate_saturation(0);
+     282             :   }
+     283             : 
+     284       94388 :   if (rate_feedback(1) > rate_saturation(1)) {
+     285           0 :     rate_feedback(1) = rate_saturation(1);
+     286       94388 :   } else if (rate_feedback(1) < -rate_saturation(1)) {
+     287           0 :     rate_feedback(1) = -rate_saturation(1);
+     288             :   }
+     289             : 
+     290       94388 :   if (rate_feedback(2) > rate_saturation(2)) {
+     291           0 :     rate_feedback(2) = rate_saturation(2);
+     292       94388 :   } 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       94388 :   mrs_msgs::HwApiAttitudeRateCmd cmd;
+     299             : 
+     300       94388 :   cmd.stamp = ros::Time::now();
+     301             : 
+     302       94388 :   cmd.body_rate.x = rate_feedback(0);
+     303       94388 :   cmd.body_rate.y = rate_feedback(1);
+     304       94388 :   cmd.body_rate.z = rate_feedback(2);
+     305             : 
+     306       94388 :   cmd.throttle = reference.throttle;
+     307             : 
+     308      188776 :   return cmd;
+     309             : }
+     310             : 
+     311             : //}
+     312             : 
+     313             : /* attitudeRateController() //{ */
+     314             : 
+     315        7958 : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+     316             :                                                                      const Eigen::Vector3d& gains) {
+     317             : 
+     318        7958 :   Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
+     319        7958 :   Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     320             : 
+     321        7958 :   Eigen::Vector3d ctrl_group_action = (des_rate - cur_rate).array() * gains.array();
+     322             : 
+     323        7958 :   mrs_msgs::HwApiControlGroupCmd cmd;
+     324             : 
+     325        7958 :   cmd.stamp = ros::Time::now();
+     326             : 
+     327        7958 :   cmd.throttle = reference.throttle;
+     328             : 
+     329        7958 :   cmd.roll  = ctrl_group_action(0);
+     330        7958 :   cmd.pitch = ctrl_group_action(1);
+     331        7958 :   cmd.yaw   = ctrl_group_action(2);
+     332             : 
+     333       15916 :   return {cmd};
+     334             : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* actuatorMixer() //{ */
+     339             : 
+     340        3972 : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
+     341             : 
+     342        3972 :   Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
+     343             : 
+     344        7944 :   Eigen::VectorXd motors = mixer * ctrl_group;
+     345             : 
+     346        3972 :   double min = motors.minCoeff();
+     347             : 
+     348        3972 :   if (min < 0.0) {
+     349           0 :     motors.array() += abs(min);
+     350             :   }
+     351             : 
+     352        3972 :   double max = motors.maxCoeff();
+     353             : 
+     354        3972 :   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        3972 :   mrs_msgs::HwApiActuatorCmd actuator_msg;
+     373             : 
+     374        3972 :   actuator_msg.stamp = ros::Time::now();
+     375             : 
+     376       19860 :   for (int i = 0; i < motors.size(); i++) {
+     377       15888 :     actuator_msg.motors.push_back(motors(i));
+     378             :   }
+     379             : 
+     380        7944 :   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:14119472.7 %
Date:2024-06-08 22:13:12Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()34
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)424
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()975
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)10193
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)136025
+
+
+ + + +
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..7c1eac4224 --- /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:14119472.7 %
Date:2024-06-08 22:13:12Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()34
mrs_uav_controllers::failsafe_controller::FailsafeController::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)107
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)10193
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)424
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)136025
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()975
+
+
+ + + +
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..a8315f9e99 --- /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:14119472.7 %
Date:2024-06-08 22:13:12Functions: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         107 : 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         107 :   nh_ = nh;
+     117             : 
+     118         107 :   common_handlers_  = common_handlers;
+     119         107 :   private_handlers_ = private_handlers;
+     120             : 
+     121         107 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123         107 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131         107 :   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         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141         214 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153         107 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158         107 :   _descend_speed_        = std::abs(_descend_speed_);
+     159         107 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161         107 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165         107 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166         107 :   shopts.nh                 = nh_;
+     167         107 :   shopts.node_name          = "FailsafeController";
+     168         107 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169         107 :   shopts.threadsafe         = true;
+     170         107 :   shopts.autostart          = true;
+     171         107 :   shopts.queue_size         = 10;
+     172         107 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174         107 :   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         107 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182         107 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186         107 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188         107 :   is_initialized_ = true;
+     189             : 
+     190         107 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           8 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           8 :   if (!last_control_output.control_output) {
+     202             : 
+     203           0 :     ROS_WARN("[FailsafeController]: activated without getting the last controller's command");
+     204             : 
+     205           0 :     return false;
+     206             : 
+     207             :   } else {
+     208             : 
+     209             :     // | -------------- calculate the initial heading ------------- |
+     210             : 
+     211           8 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213          16 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           8 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           8 :       ROS_INFO("[FailsafeController]: activated with heading = %.2f rad", heading_setpoint_);
+     218             : 
+     219             :     } else {
+     220             : 
+     221           0 :       ROS_ERROR("[FailsafeController]: missing orientation from HW API, activated with heading = 0 rad");
+     222             : 
+     223           0 :       heading_setpoint_ = 0;
+     224             :     }
+     225             : 
+     226           8 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           8 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           7 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           1 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           8 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           8 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           8 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           8 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           8 :   first_iteration_ = true;
+     243             : 
+     244           8 :   is_active_ = true;
+     245             : 
+     246           8 :   return true;
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* deactivate() //{ */
+     252             : 
+     253          34 : void FailsafeController::deactivate(void) {
+     254             : 
+     255          34 :   is_active_           = false;
+     256          34 :   first_iteration_     = false;
+     257          34 :   uav_mass_difference_ = 0;
+     258             : 
+     259          34 :   ROS_INFO("[FailsafeController]: deactivated");
+     260          34 : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* updateInactive() //{ */
+     265             : 
+     266      136025 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268      136025 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270      136025 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272      136025 :   first_iteration_ = false;
+     273      136025 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279       10193 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282       30579 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283       30579 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286       20386 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288       10193 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291       10193 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295       10193 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301       10193 :   if (first_iteration_) {
+     302           8 :     dt               = 0.01;
+     303           8 :     first_iteration_ = false;
+     304             :   } else {
+     305       10185 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308       10193 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310       10193 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312       10193 :   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       10193 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317       10193 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323       20386 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325       10193 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327       10193 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329       10193 :   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       10193 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340       10193 :   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       10193 :   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       10193 :   if (highest_modality.value() == common::VELOCITY_HDG_RATE) {
+     366             : 
+     367         958 :     mrs_msgs::HwApiVelocityHdgRateCmd vel_cmd;
+     368             : 
+     369         479 :     vel_cmd.header.stamp = ros::Time::now();
+     370             : 
+     371         479 :     vel_cmd.velocity.x   = 0;
+     372         479 :     vel_cmd.velocity.y   = 0;
+     373         479 :     vel_cmd.velocity.z   = -_descend_speed_;
+     374         479 :     vel_cmd.heading_rate = 0.0;
+     375             : 
+     376         479 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning velocity+hdg rate output");
+     377         479 :     control_output.control_output = vel_cmd;
+     378         479 :     return control_output;
+     379             :   }
+     380             : 
+     381             :   // --------------------------------------------------------------
+     382             :   // |                     acceleration output                    |
+     383             :   // --------------------------------------------------------------
+     384             : 
+     385        9714 :   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        9714 :   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        9714 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423        9714 :   attitude_cmd.stamp       = ros::Time::now();
+     424        9714 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425        9714 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427        9714 :   if (highest_modality.value() == common::ATTITUDE) {
+     428        5536 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude output");
+     429        5536 :     control_output.control_output = attitude_cmd;
+     430        5536 :     return control_output;
+     431             :   }
+     432             : 
+     433             :   // --------------------------------------------------------------
+     434             :   // |                      attitude control                      |
+     435             :   // --------------------------------------------------------------
+     436             : 
+     437        4178 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438        4178 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439        4178 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441        4178 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443        4178 :   if (highest_modality.value() == common::ATTITUDE_RATE) {
+     444        1393 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude rate output");
+     445        1393 :     control_output.control_output = attitude_rate_command;
+     446        1393 :     return control_output;
+     447             :   }
+     448             : 
+     449             :   // --------------------------------------------------------------
+     450             :   // |                    attitude rate control                   |
+     451             :   // --------------------------------------------------------------
+     452             : 
+     453        2785 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455        2785 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457        2785 :   if (highest_modality.value() == common::CONTROL_GROUP) {
+     458        1393 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning control group output");
+     459        1393 :     control_output.control_output = control_group_command;
+     460        1393 :     return control_output;
+     461             :   }
+     462             : 
+     463             :   // --------------------------------------------------------------
+     464             :   // |                            mixer                           |
+     465             :   // --------------------------------------------------------------
+     466             : 
+     467        2784 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469        1392 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470        1392 :   control_output.control_output = actuator_cmd;
+     471        1392 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478         975 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480         975 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482         975 :   controller_status.active = is_active_;
+     483             : 
+     484         975 :   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         424 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         424 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         424 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         424 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         848 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         424 :   res.success = true;
+     518         424 :   res.message = "constraints updated";
+     519             : 
+     520         424 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           8 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           8 :     return mrs_lib::AttitudeConverter(quaternion).getHeading();
+     531             :   }
+     532           0 :   catch (...) {
+     533             :   }
+     534             : 
+     535             :   try {
+     536           0 :     return mrs_lib::AttitudeConverter(quaternion).getYaw();
+     537             :   }
+     538           0 :   catch (...) {
+     539             :   }
+     540             : 
+     541           0 :   return 0;
+     542             : }
+     543             : 
+     544             : //}
+     545             : 
+     546             : }  // namespace failsafe_controller
+     547             : 
+     548             : }  // namespace mrs_uav_controllers
+     549             : 
+     550             : #include <pluginlib/class_list_macros.h>
+     551         107 : 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..9a6ce12e01ca80cf1717e1ba1ccb451085acac55 GIT binary patch literal 1823 zcmV+)2jKXLP)e=oRz^c!hccd;hDv%5TC?OaR;GEW6w3*vv;rK0*l04)ZwA^Yr;M z^lBeG4LV;z2Ej{FY1dO9Jgno~MOTbSD4OXyFO3BGUH7niKSO&&O-F`#y7&Cku1DaR zF0&C1WgOvm=1~nI*V|z%Q}`OsBFMeuqiT}+ekQCy0>hj#HC>}_NB3G0>GD|q(PZC< z5HSx7Vhf)m$EM1P!8258(9a-FF=A>{k)r4aG5RD&Gl0xdhHhj%P>60UI#65+ek3jg z1zle?Ynf|7@l+Tg&BoY|lYDqHm-I8HxVWPdVoyPS*T9Z0mxRk`l^uao2xa}p-9kAc z#b_x3XdxdBV;GTOFHZ3JSc6Dw84I>+5Kq?;chNRoJK1NZT40d`ag5e{J#Y_IJ-HyI zxq{7R@(5R!NK7`i#w^U9X~yJf2~yLQOZ|twx7NiovvRdhh1fiK6{EoxiP3e1F^K(l zn(2-Nn;{7LpBaOJfOUNoq$NLR1leubl-g8^8@Ei?B|)de zqWn8rPgH8W^@bZWb<7yA%Tu5jllETQO7vJQxe3yYHKX` zdcC8zpMUrF@%dxg>ug5w^V#vD+wi)QaDw~>gZ<=O*OFcr4IVXtpupE=`ngHC<~`Mc zJrTMr#VwSAs)LSWeDu*ixtv-lDY!a9&rs>O3~{A$Rwu~F{*55J z-L)_5c_f}mmQ}(rrDO)zkh#gP06!Yn>o{c=Atnp^;a<$YL$BuV_rP1NMHv7WahR%( zLdoVMz~p#@%E4@M96DfqWJJ$nJ0qK+}+u)Z0>4U ztrOVYa9+Z2k9Zt!QjnMAifD!y7VW>0XSgmb?g5m-f+))^(r3`wQ;z#-*Sx&>MC1*o z){dp|OSV#_R4V$OMq~?SyY^k$m*6Bl69=XlC3b{_pmpR9h65)?{9}2~?X~8NDWopc z?@vDx^V;>t7VNgaW+&`yQn0(e@sOk&E%KG)5Xn~i+E(RS0jS)LcO4hl@I%q;A*29z zK6=2H>405&J!(rXe%ByUKp#c=Aet#M%N1m#T+;#7Ku;~CSm5~;8TM{i*zF)L5;?!! zGwCpu4Bt`kT?8ydP?3@#>tvBRAPeA?u-uu7y#BaRE8$jqUPuK3%*O+~-`%&Gp9fw(zDtV! z2CurUWbP}SOazbayR8((o-lKxX^UOBhqtakeAwr|bm5~7o++^LXchW^kI%n4Q?hV(Hj zC`VkV5>l-osUsQ;Rpyot2JgaG|DwykiDy*aF&5Dcg6z6`!d@4{!yVu7>?biCMe&An z*FqHkpTux7I9Ch8(H8{I=R_3m8^g~J{y+>5QM`qZc}D*0d_0Hn5iCLeqkMcu9*$;6 zajC1>u`dgi0^qLWVkMZoP*4TLbjhtQHI=t}*ExkbRW9Tdsg}>nDg1xE@l`&SDl_hR zB)Asuuwubg|7|aQ0{Z02?5ow0k62#0RwT&b;{Ds#3%95&~ZZK`UW4V%gVxE zY`9<#_q;G5c4N=_*V$tjglX{I)iqaiuB(UV_qh5cDV&$qBYzR-%7c%(2sHLN+CN*o z6vgvZzNyHjTv^XlA=-$N7nC5DH3NbVB88d4UG4&*(tp}^_xZrwQwVlQxBoJE2MbZ= zIe + + + + + + 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:1759217480.9 %
Date:2024-06-08 22:13:12Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
72.7%72.7%
+ 		72.7 %141 / 19481.8 %9 / 11
<unnamed>72.7 %141 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
se3_controller.cpp +
80.4%80.4%
+ 		80.4 %623 / 77588.2 %15 / 17
<unnamed>80.4 %623 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
<unnamed>81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-detail-sort-l.html b/mrs_uav_controllers/src/index-detail-sort-l.html new file mode 100644 index 0000000000..0da6ad315b --- /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:1759217480.9 %
Date:2024-06-08 22:13:12Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
72.7%72.7%
+ 		72.7 %141 / 19481.8 %9 / 11
<unnamed>72.7 %141 / 19481.8 %9 / 11
se3_controller.cpp +
80.4%80.4%
+ 		80.4 %623 / 77588.2 %15 / 17
<unnamed>80.4 %623 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
<unnamed>81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-detail.html b/mrs_uav_controllers/src/index-detail.html new file mode 100644 index 0000000000..ee31683ceb --- /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:1759217480.9 %
Date:2024-06-08 22:13:12Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
<unnamed>84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
72.7%72.7%
+ 		72.7 %141 / 19481.8 %9 / 11
<unnamed>72.7 %141 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
<unnamed>81.2 %727 / 89588.9 %16 / 18
se3_controller.cpp +
80.4%80.4%
+ 		80.4 %623 / 77588.2 %15 / 17
<unnamed>80.4 %623 / 77588.2 %15 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-sort-f.html b/mrs_uav_controllers/src/index-sort-f.html new file mode 100644 index 0000000000..c81582e709 --- /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:1759217480.9 %
Date:2024-06-08 22:13:12Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
72.7%72.7%
+ 		72.7 %141 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
se3_controller.cpp +
80.4%80.4%
+ 		80.4 %623 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-sort-l.html b/mrs_uav_controllers/src/index-sort-l.html new file mode 100644 index 0000000000..5899913be3 --- /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:1759217480.9 %
Date:2024-06-08 22:13:12Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
72.7%72.7%
+ 		72.7 %141 / 19481.8 %9 / 11
se3_controller.cpp +
80.4%80.4%
+ 		80.4 %623 / 77588.2 %15 / 17
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index.html b/mrs_uav_controllers/src/index.html new file mode 100644 index 0000000000..646d65e488 --- /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:1759217480.9 %
Date:2024-06-08 22:13:12Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
84.3%84.3%
+ 		84.3 %134 / 159100.0 %9 / 9
failsafe_controller.cpp +
72.7%72.7%
+ 		72.7 %141 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
81.2%81.2%
+ 		81.2 %727 / 89588.9 %16 / 18
se3_controller.cpp +
80.4%80.4%
+ 		80.4 %623 / 77588.2 %15 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html new file mode 100644 index 0000000000..b98afa63ae --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-06-08 22:13:12Functions: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&)13
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()102
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()131
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)161
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)424
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)500
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)145718
+
+
+ + + +
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..bef0714ad2 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-06-08 22:13:12Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_controllers::mpc_controller::MpcController::deactivate()173
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>)214
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)214
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)226
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)283
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&)630
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)848
mrs_uav_controllers::mpc_controller::MpcController::getStatus()14847
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)16050
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&)69033
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)69663
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)69946
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)126912
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)222490
+
+
+ + + +
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..4e33dfc1f5 --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - mpc_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:72789581.2 %
Date:2024-06-08 22:13:12Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_controllers::mpc_controller::MpcController::deactivate()173
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>)214
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)16050
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)214
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)69946
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)848
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)222490
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)69663
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&)630
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)126912
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)283
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
mrs_uav_controllers::mpc_controller::MpcController::MPC(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, mrs_uav_controllers::common::CONTROL_OUTPUT const&)69033
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)226
mrs_uav_controllers::mpc_controller::MpcController::getStatus()14847
+
+
+ + + +
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..6ddb648fa5 --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.html @@ -0,0 +1,2217 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - mpc_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:72789581.2 %
Date:2024-06-08 22:13:12Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::se3_controller::Se3Controller::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::se3_controller::Se3Controller::resetDisturbanceEstimators()0
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()23
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)27
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)215
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)249
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)424
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2916
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&)3017
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4686
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&)25695
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28712
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28961
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)65226
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)117257
+
+
+ + + +
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..647f5ebe36 --- /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:62377580.4 %
Date:2024-06-08 22:13:12Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()23
mrs_uav_controllers::se3_controller::Se3Controller::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)107
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4686
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)215
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28961
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&)25695
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)424
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)117257
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28712
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&)3017
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)65226
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)249
mrs_uav_controllers::se3_controller::Se3Controller::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::se3_controller::Se3Controller::resetDisturbanceEstimators()0
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)27
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2916
+
+
+ + + +
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..74ab5720c7 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.html @@ -0,0 +1,1930 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - se3_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:62377580.4 %
Date:2024-06-08 22:13:12Functions: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         107 : 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         107 :   nh_ = nh;
+     217             : 
+     218         107 :   common_handlers_  = common_handlers;
+     219         107 :   private_handlers_ = private_handlers;
+     220             : 
+     221         107 :   _uav_mass_ = common_handlers->getMass();
+     222             : 
+     223         107 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // | ---------- loading params using the parent's nh ---------- |
+     226             : 
+     227         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     228             : 
+     229         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     230             : 
+     231         107 :   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         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
+     239         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
+     240             : 
+     241         214 :   const std::string yaml_namespace = "mrs_uav_controllers/se3_controller/";
+     242             : 
+     243             :   // lateral gains
+     244         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kp", gains_.kpxy);
+     245         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kv", gains_.kvxy);
+     246         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/ka", gains_.kaxy);
+     247             : 
+     248         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw", gains_.kiwxy);
+     249         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib", gains_.kibxy);
+     250             : 
+     251             :   // | ------------------------- rampup ------------------------- |
+     252             : 
+     253         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/enabled", _rampup_enabled_);
+     254         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/speed", _rampup_speed_);
+     255             : 
+     256             :   // height gains
+     257         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kp", gains_.kpz);
+     258         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kv", gains_.kvz);
+     259         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/ka", gains_.kaz);
+     260             : 
+     261             :   // attitude gains
+     262         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_roll_pitch", gains_.kq_roll_pitch);
+     263         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_yaw", gains_.kq_yaw);
+     264             : 
+     265             :   // attitude rate gains
+     266         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_roll_pitch", gains_.kw_roll_pitch);
+     267         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_yaw", gains_.kw_yaw);
+     268             : 
+     269             :   // mass estimator
+     270         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km", gains_.km);
+     271         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
+     272             : 
+     273             :   // integrator limits
+     274         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
+     275         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib_lim", gains_.kibxy_lim);
+     276             : 
+     277             :   // constraints
+     278         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     279         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     280             : 
+     281         107 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     282             : 
+     283         107 :   if (_tilt_angle_failsafe_enabled_ && std::abs(_tilt_angle_failsafe_) < 1e-3) {
+     284           0 :     ROS_ERROR("[Se3Controller]: constraints/tilt_angle_failsafe/enabled = 'TRUE' but the limit is too low");
+     285           0 :     return false;
+     286             :   }
+     287             : 
+     288         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/throttle_saturation", _throttle_saturation_);
+     289             : 
+     290             :   // gain filtering
+     291         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     292         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     293         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/rate", _gain_filtering_rate_);
+     294         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     295             : 
+     296             :   // output mode
+     297         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/preferred_output", drs_params_.preferred_output_mode);
+     298             : 
+     299         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rotation_matrix", drs_params_.rotation_type);
+     300             : 
+     301             :   // angular rate feed forward
+     302         214 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/parasitic_pitch_roll",
+     303         107 :                                             drs_params_.pitch_roll_heading_rate_compensation);
+     304         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     305             : 
+     306             :   // | ------------------- position pid params ------------------ |
+     307             : 
+     308         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     309         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     310         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     311             : 
+     312         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     313         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     314         107 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     315             : 
+     316             :   // | ------------------ finish loading params ----------------- |
+     317             : 
+     318         107 :   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         107 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     326         107 :         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         107 :   uav_mass_difference_ = 0;
+     333         107 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     334         107 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     335             : 
+     336             :   // | --------------- dynamic reconfigure server --------------- |
+     337             : 
+     338         107 :   drs_params_.kpxy             = gains_.kpxy;
+     339         107 :   drs_params_.kvxy             = gains_.kvxy;
+     340         107 :   drs_params_.kaxy             = gains_.kaxy;
+     341         107 :   drs_params_.kiwxy            = gains_.kiwxy;
+     342         107 :   drs_params_.kibxy            = gains_.kibxy;
+     343         107 :   drs_params_.kpz              = gains_.kpz;
+     344         107 :   drs_params_.kvz              = gains_.kvz;
+     345         107 :   drs_params_.kaz              = gains_.kaz;
+     346         107 :   drs_params_.kq_roll_pitch    = gains_.kq_roll_pitch;
+     347         107 :   drs_params_.kq_yaw           = gains_.kq_yaw;
+     348         107 :   drs_params_.kiwxy_lim        = gains_.kiwxy_lim;
+     349         107 :   drs_params_.kibxy_lim        = gains_.kibxy_lim;
+     350         107 :   drs_params_.km               = gains_.km;
+     351         107 :   drs_params_.km_lim           = gains_.km_lim;
+     352         107 :   drs_params_.jerk_feedforward = true;
+     353             : 
+     354         107 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     355         107 :   drs_->updateConfig(drs_params_);
+     356         107 :   Drs_t::CallbackType f = boost::bind(&Se3Controller::callbackDrs, this, _1, _2);
+     357         107 :   drs_->setCallback(f);
+     358             : 
+     359             :   // | ------------------------- timers ------------------------- |
+     360             : 
+     361         107 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &Se3Controller::timerGains, this, false, false);
+     362             : 
+     363             :   // | ---------------------- position pid ---------------------- |
+     364             : 
+     365         107 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     366         107 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     367         107 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     368         107 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     369             : 
+     370             :   // | ------------------------ profiler ------------------------ |
+     371             : 
+     372         107 :   profiler_ = mrs_lib::Profiler(common_handlers_->parent_nh, "Se3Controller", _profiler_enabled_);
+     373             : 
+     374             :   // | ----------------------- finish init ---------------------- |
+     375             : 
+     376         107 :   ROS_INFO("[Se3Controller]: initialized");
+     377             : 
+     378         107 :   is_initialized_ = true;
+     379             : 
+     380         107 :   return true;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : /* //{ activate() */
+     386             : 
+     387          27 : bool Se3Controller::activate(const ControlOutput& last_control_output) {
+     388             : 
+     389          27 :   activation_control_output_ = last_control_output;
+     390             : 
+     391          27 :   double activation_mass = _uav_mass_;
+     392             : 
+     393          27 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     394           1 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     395           1 :     activation_mass += uav_mass_difference_;
+     396           1 :     ROS_INFO("[Se3Controller]: setting mass difference from the last control output: %.2f kg", uav_mass_difference_);
+     397             :   }
+     398             : 
+     399          27 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     400             : 
+     401          27 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     402           1 :     Ib_b_(0) = -activation_control_output_.diagnostics.disturbance_bx_b;
+     403           1 :     Ib_b_(1) = -activation_control_output_.diagnostics.disturbance_by_b;
+     404             : 
+     405           1 :     Iw_w_(0) = -activation_control_output_.diagnostics.disturbance_wx_w;
+     406           1 :     Iw_w_(1) = -activation_control_output_.diagnostics.disturbance_wy_w;
+     407             : 
+     408           1 :     ROS_INFO(
+     409             :         "[Se3Controller]: setting disturbances from the last control output: Ib_b_: %.2f, %.2f N, Iw_w_: "
+     410             :         "%.2f, %.2f N",
+     411             :         Ib_b_(0), Ib_b_(1), Iw_w_(0), Iw_w_(1));
+     412             :   }
+     413             : 
+     414             :   // did the last controller use manual throttle control?
+     415          27 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     416             : 
+     417             :   // rampup check
+     418          27 :   if (_rampup_enabled_ && throttle_last_controller) {
+     419             : 
+     420          14 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     421             : 
+     422          14 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     423             : 
+     424          14 :     if (throttle_difference > 0) {
+     425           3 :       rampup_direction_ = 1;
+     426          11 :     } else if (throttle_difference < 0) {
+     427           0 :       rampup_direction_ = -1;
+     428             :     } else {
+     429          11 :       rampup_direction_ = 0;
+     430             :     }
+     431             : 
+     432          14 :     ROS_INFO("[Se3Controller]: activating rampup with initial throttle: %.4f, target: %.4f", throttle_last_controller.value(), hover_throttle);
+     433             : 
+     434          14 :     rampup_active_     = true;
+     435          14 :     rampup_start_time_ = ros::Time::now();
+     436          14 :     rampup_last_time_  = ros::Time::now();
+     437          14 :     rampup_throttle_   = throttle_last_controller.value();
+     438             : 
+     439          14 :     rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
+     440             :   }
+     441             : 
+     442          27 :   first_iteration_ = true;
+     443          27 :   mute_gains_      = true;
+     444             : 
+     445          27 :   timer_gains_.start();
+     446             : 
+     447             :   // | ------------------ finish the activation ----------------- |
+     448             : 
+     449          27 :   ROS_INFO("[Se3Controller]: activated");
+     450             : 
+     451          27 :   is_active_ = true;
+     452             : 
+     453          27 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* //{ deactivate() */
+     459             : 
+     460          23 : void Se3Controller::deactivate(void) {
+     461             : 
+     462          23 :   is_active_           = false;
+     463          23 :   first_iteration_     = false;
+     464          23 :   uav_mass_difference_ = 0;
+     465             : 
+     466          23 :   timer_gains_.stop();
+     467             : 
+     468          23 :   ROS_INFO("[Se3Controller]: deactivated");
+     469          23 : }
+     470             : 
+     471             : //}
+     472             : 
+     473             : /* updateInactive() //{ */
+     474             : 
+     475      117257 : void Se3Controller::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command) {
+     476             : 
+     477      117257 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     478             : 
+     479      117257 :   last_update_time_ = uav_state.header.stamp;
+     480             : 
+     481      117257 :   first_iteration_ = false;
+     482      117257 : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ updateWhenActive() */
+     487             : 
+     488       28961 : Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+     489             : 
+     490       86883 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     491       86883 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     492             : 
+     493       57922 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     494             : 
+     495       28961 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     496             : 
+     497       28961 :   last_control_output_.desired_heading_rate          = {};
+     498       28961 :   last_control_output_.desired_orientation           = {};
+     499       28961 :   last_control_output_.desired_unbiased_acceleration = {};
+     500       28961 :   last_control_output_.control_output                = {};
+     501             : 
+     502             :   // | -------------------- calculate the dt -------------------- |
+     503             : 
+     504             :   double dt;
+     505             : 
+     506       28961 :   if (first_iteration_) {
+     507          27 :     dt               = 0.01;
+     508          27 :     first_iteration_ = false;
+     509             :   } else {
+     510       28934 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     511             :   }
+     512             : 
+     513       28961 :   last_update_time_ = uav_state.header.stamp;
+     514             : 
+     515       28961 :   if (std::abs(dt) < 0.001) {
+     516             : 
+     517           0 :     ROS_DEBUG("[Se3Controller]: the last odometry message came too close (%.2f s)!", dt);
+     518             : 
+     519           0 :     dt = 0.01;
+     520             :   }
+     521             : 
+     522             :   // | ----------- obtain the lowest possible modality ---------- |
+     523             : 
+     524       28961 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     525             : 
+     526       28961 :   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       28961 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     536       20783 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude rate output");
+     537       20783 :     lowest_modality = common::ATTITUDE_RATE;
+     538        8178 :   } 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        8178 :   } 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        8178 :   } 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       28961 :   switch (lowest_modality.value()) {
+     550             : 
+     551         249 :     case common::POSITION: {
+     552         249 :       positionPassthrough(uav_state, tracker_command);
+     553         249 :       break;
+     554             :     }
+     555             : 
+     556        1116 :     case common::VELOCITY_HDG: {
+     557        1116 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     558        1116 :       break;
+     559             :     }
+     560             : 
+     561        1901 :     case common::VELOCITY_HDG_RATE: {
+     562        1901 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     563        1901 :       break;
+     564             :     }
+     565             : 
+     566         444 :     case common::ACCELERATION_HDG: {
+     567         444 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     568         444 :       break;
+     569             :     }
+     570             : 
+     571         596 :     case common::ACCELERATION_HDG_RATE: {
+     572         596 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     573         596 :       break;
+     574             :     }
+     575             : 
+     576        1261 :     case common::ATTITUDE: {
+     577        1261 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE);
+     578        1261 :       break;
+     579             :     }
+     580             : 
+     581       20783 :     case common::ATTITUDE_RATE: {
+     582       20783 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     583       20783 :       break;
+     584             :     }
+     585             : 
+     586        1308 :     case common::CONTROL_GROUP: {
+     587        1308 :       SE3Controller(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     588        1308 :       break;
+     589             :     }
+     590             : 
+     591        1303 :     case common::ACTUATORS_CMD: {
+     592        1303 :       SE3Controller(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     593        1303 :       break;
+     594             :     }
+     595             : 
+     596       28961 :     default: {
+     597             :     }
+     598             :   }
+     599             : 
+     600       28961 :   return last_control_output_;
+     601             : }
+     602             : 
+     603             : //}
+     604             : 
+     605             : /* //{ getStatus() */
+     606             : 
+     607        2916 : const mrs_msgs::ControllerStatus Se3Controller::getStatus() {
+     608             : 
+     609        2916 :   mrs_msgs::ControllerStatus controller_status;
+     610             : 
+     611        2916 :   controller_status.active = is_active_;
+     612             : 
+     613        2916 :   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         424 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr Se3Controller::setConstraints([
+     674             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+     675             : 
+     676         424 :   if (!is_initialized_) {
+     677           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     678             :   }
+     679             : 
+     680         424 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     681             : 
+     682         424 :   ROS_INFO("[Se3Controller]: updating constraints");
+     683             : 
+     684         848 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     685         424 :   res.success = true;
+     686         424 :   res.message = "constraints updated";
+     687             : 
+     688         424 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     689             : }
+     690             : 
+     691             : //}
+     692             : 
+     693             : // --------------------------------------------------------------
+     694             : // |                         controllers                        |
+     695             : // --------------------------------------------------------------
+     696             : 
+     697             : /* SE3Controller() //{ */
+     698             : 
+     699       25695 : 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       51390 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     703       25695 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     704       25695 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     705             : 
+     706             :   // | ----------------- get the current heading ---------------- |
+     707             : 
+     708       25695 :   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       25695 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     719       25695 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     720       25695 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     721             : 
+     722       25695 :   if (tracker_command.use_position_vertical || tracker_command.use_position_horizontal) {
+     723             : 
+     724       25695 :     if (tracker_command.use_position_horizontal) {
+     725       25695 :       Rp(0) = tracker_command.position.x;
+     726       25695 :       Rp(1) = tracker_command.position.y;
+     727             :     } else {
+     728           0 :       Rv(0) = 0;
+     729           0 :       Rv(1) = 0;
+     730             :     }
+     731             : 
+     732       25695 :     if (tracker_command.use_position_vertical) {
+     733       25695 :       Rp(2) = tracker_command.position.z;
+     734             :     } else {
+     735           0 :       Rv(2) = 0;
+     736             :     }
+     737             :   }
+     738             : 
+     739       25695 :   if (tracker_command.use_velocity_horizontal) {
+     740       25695 :     Rv(0) = tracker_command.velocity.x;
+     741       25695 :     Rv(1) = tracker_command.velocity.y;
+     742             :   } else {
+     743           0 :     Rv(0) = 0;
+     744           0 :     Rv(1) = 0;
+     745             :   }
+     746             : 
+     747       25695 :   if (tracker_command.use_velocity_vertical) {
+     748       25695 :     Rv(2) = tracker_command.velocity.z;
+     749             :   } else {
+     750           0 :     Rv(2) = 0;
+     751             :   }
+     752             : 
+     753       25695 :   if (tracker_command.use_acceleration) {
+     754       25695 :     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       25695 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     764       25695 :   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       25695 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     768             : 
+     769             :   // Ow - UAV angular rate
+     770       25695 :   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       25695 :   Eigen::Vector3d Ep(0, 0, 0);
+     776             : 
+     777       25695 :   if (tracker_command.use_position_horizontal || tracker_command.use_position_vertical) {
+     778       25695 :     Ep = Rp - Op;
+     779             :   }
+     780             : 
+     781             :   // velocity control error
+     782       25695 :   Eigen::Vector3d Ev(0, 0, 0);
+     783             : 
+     784       25695 :   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       25695 :     Ev = Rv - Ov;
+     787             :   }
+     788             : 
+     789             :   // | --------------------- load the gains --------------------- |
+     790             : 
+     791       25695 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+     792             : 
+     793       25695 :   Eigen::Vector3d Ka(0, 0, 0);
+     794       25695 :   Eigen::Array3d  Kp(0, 0, 0);
+     795       25695 :   Eigen::Array3d  Kv(0, 0, 0);
+     796       25695 :   Eigen::Array3d  Kq(0, 0, 0);
+     797       25695 :   Eigen::Array3d  Kw(0, 0, 0);
+     798             : 
+     799             :   {
+     800       25695 :     std::scoped_lock lock(mutex_gains_);
+     801             : 
+     802       25695 :     if (tracker_command.use_position_horizontal) {
+     803       25695 :       Kp(0) = gains.kpxy;
+     804       25695 :       Kp(1) = gains.kpxy;
+     805             :     } else {
+     806           0 :       Kp(0) = 0;
+     807           0 :       Kp(1) = 0;
+     808             :     }
+     809             : 
+     810       25695 :     if (tracker_command.use_position_vertical) {
+     811       25695 :       Kp(2) = gains.kpz;
+     812             :     } else {
+     813           0 :       Kp(2) = 0;
+     814             :     }
+     815             : 
+     816       25695 :     if (tracker_command.use_velocity_horizontal) {
+     817       25695 :       Kv(0) = gains.kvxy;
+     818       25695 :       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       25695 :     if (tracker_command.use_velocity_vertical || tracker_command.use_position_vertical) {
+     826       25695 :       Kv(2) = gains.kvz;
+     827             :     } else {
+     828           0 :       Kv(2) = 0;
+     829             :     }
+     830             : 
+     831       25695 :     if (tracker_command.use_acceleration) {
+     832       25695 :       Ka << gains.kaxy, gains.kaxy, gains.kaz;
+     833             :     } else {
+     834           0 :       Ka << 0, 0, 0;
+     835             :     }
+     836             : 
+     837       25695 :     if (!tracker_command.use_attitude_rate) {
+     838       25695 :       Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+     839             :     }
+     840             : 
+     841       25695 :     Kw(0) = gains.kw_roll_pitch;
+     842       25695 :     Kw(1) = gains.kw_roll_pitch;
+     843       25695 :     Kw(2) = gains.kw_yaw;
+     844             :   }
+     845             : 
+     846       25695 :   Kp = Kp * (_uav_mass_ + uav_mass_difference_);
+     847       25695 :   Kv = Kv * (_uav_mass_ + uav_mass_difference_);
+     848             : 
+     849             :   // | --------------- desired orientation matrix --------------- |
+     850             : 
+     851             :   // get body integral in the world frame
+     852             : 
+     853       25695 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+     854             : 
+     855             :   {
+     856       51390 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+     857             : 
+     858       25695 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+     859       25695 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+     860       25695 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+     861       25695 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+     862       25695 :     Ib_b_stamped.vector.z        = 0;
+     863             : 
+     864       51390 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+     865             : 
+     866       25695 :     if (res) {
+     867       25695 :       Ib_w(0) = res.value().vector.x;
+     868       25695 :       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       25695 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+     877             : 
+     878       25695 :   Eigen::Vector3d feed_forward      = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+     879       25695 :   Eigen::Vector3d position_feedback = Kp * Ep.array();
+     880       25695 :   Eigen::Vector3d velocity_feedback = Kv * Ev.array();
+     881       25695 :   Eigen::Vector3d integral_feedback;
+     882             :   {
+     883       25695 :     std::scoped_lock lock(mutex_integrals_);
+     884             : 
+     885       25695 :     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       51390 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+     900             : 
+     901       25695 :     Eigen::Vector3d integration_switch(1, 1, 0);
+     902             : 
+     903             :     // integrate the world error
+     904       25695 :     if (tracker_command.use_position_horizontal) {
+     905       25695 :       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       25695 :     bool world_integral_saturated = false;
+     912       25695 :     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       25695 :     } else if (Iw_w_(0) > gains.kiwxy_lim) {
+     916           0 :       Iw_w_(0)                 = gains.kiwxy_lim;
+     917           0 :       world_integral_saturated = true;
+     918       25695 :     } 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       25695 :     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       25695 :     world_integral_saturated = false;
+     929       25695 :     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       25695 :     } else if (Iw_w_(1) > gains.kiwxy_lim) {
+     933           0 :       Iw_w_(1)                 = gains.kiwxy_lim;
+     934           0 :       world_integral_saturated = true;
+     935       25695 :     } 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       25695 :     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       51390 :     std::scoped_lock lock(mutex_gains_);
+     955             : 
+     956       25695 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+     957       25695 :     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       51390 :       geometry_msgs::Vector3Stamped Ep_stamped;
+     963             : 
+     964       25695 :       Ep_stamped.header.stamp    = ros::Time::now();
+     965       25695 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+     966       25695 :       Ep_stamped.vector.x        = Ep(0);
+     967       25695 :       Ep_stamped.vector.y        = Ep(1);
+     968       25695 :       Ep_stamped.vector.z        = Ep(2);
+     969             : 
+     970       77085 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+     971             : 
+     972       25695 :       if (res) {
+     973       25695 :         Ep_fcu_untilted(0) = res.value().vector.x;
+     974       25695 :         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       51390 :       geometry_msgs::Vector3Stamped Ev_stamped;
+     983             : 
+     984       25695 :       Ev_stamped.header.stamp    = ros::Time::now();
+     985       25695 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+     986       25695 :       Ev_stamped.vector.x        = Ev(0);
+     987       25695 :       Ev_stamped.vector.y        = Ev(1);
+     988       25695 :       Ev_stamped.vector.z        = Ev(2);
+     989             : 
+     990       77085 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+     991             : 
+     992       25695 :       if (res) {
+     993       25695 :         Ev_fcu_untilted(0) = res.value().vector.x;
+     994       25695 :         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       25695 :     if (tracker_command.use_position_horizontal) {
+    1002       25695 :       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       25695 :     bool body_integral_saturated = false;
+    1009       25695 :     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       25695 :     } else if (Ib_b_(0) > gains.kibxy_lim) {
+    1013           0 :       Ib_b_(0)                = gains.kibxy_lim;
+    1014           0 :       body_integral_saturated = true;
+    1015       25695 :     } 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       25695 :     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       25695 :     body_integral_saturated = false;
+    1026       25695 :     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       25695 :     } else if (Ib_b_(1) > gains.kibxy_lim) {
+    1030           0 :       Ib_b_(1)                = gains.kibxy_lim;
+    1031           0 :       body_integral_saturated = true;
+    1032       25695 :     } 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       25695 :     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       25695 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1045             : 
+    1046        1040 :     Eigen::Vector3d des_acc = (position_feedback + velocity_feedback + integral_feedback) / total_mass + Ra;
+    1047             : 
+    1048        1040 :     if (output_modality == common::ACCELERATION_HDG) {
+    1049             : 
+    1050         888 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1051             : 
+    1052         444 :       cmd.acceleration.x = des_acc(0);
+    1053         444 :       cmd.acceleration.y = des_acc(1);
+    1054         444 :       cmd.acceleration.z = des_acc(2);
+    1055             : 
+    1056         444 :       cmd.heading = tracker_command.heading;
+    1057             : 
+    1058         444 :       last_control_output_.control_output = cmd;
+    1059             : 
+    1060             :     } else {
+    1061             : 
+    1062         596 :       double des_hdg_ff = 0;
+    1063             : 
+    1064         596 :       if (tracker_command.use_heading_rate) {
+    1065         596 :         des_hdg_ff = tracker_command.heading_rate;
+    1066             :       }
+    1067             : 
+    1068        1192 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1069             : 
+    1070         596 :       cmd.acceleration.x = des_acc(0);
+    1071         596 :       cmd.acceleration.y = des_acc(1);
+    1072         596 :       cmd.acceleration.z = des_acc(2);
+    1073             : 
+    1074         596 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1075             : 
+    1076         596 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1077             : 
+    1078         596 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1079             : 
+    1080         596 :       cmd.heading_rate = des_hdg_rate;
+    1081             : 
+    1082         596 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1083             : 
+    1084         596 :       last_control_output_.control_output = cmd;
+    1085             :     }
+    1086             : 
+    1087             :     // | -------------- unbiased desired acceleration ------------- |
+    1088             : 
+    1089        1040 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1090             : 
+    1091             :     {
+    1092             : 
+    1093        1040 :       Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1094             : 
+    1095        2080 :       geometry_msgs::Vector3Stamped world_accel;
+    1096             : 
+    1097        1040 :       world_accel.header.stamp    = ros::Time::now();
+    1098        1040 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1099        1040 :       world_accel.vector.x        = unbiased_des_acc_world(0);
+    1100        1040 :       world_accel.vector.y        = unbiased_des_acc_world(1);
+    1101        1040 :       world_accel.vector.z        = unbiased_des_acc_world(2);
+    1102             : 
+    1103        3120 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1104             : 
+    1105        1040 :       if (res) {
+    1106        1040 :         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        1040 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1112             : 
+    1113             :     // | ----------------- fill in the diagnostics ---------------- |
+    1114             : 
+    1115        1040 :     last_control_output_.diagnostics.ramping_up = false;
+    1116             : 
+    1117        1040 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1118        1040 :     last_control_output_.diagnostics.mass_difference = 0;
+    1119        1040 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1120             : 
+    1121        1040 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1122             : 
+    1123        1040 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1124        1040 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1125             : 
+    1126        1040 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1127        1040 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1128             : 
+    1129        1040 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1130        1040 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1131             : 
+    1132        1040 :     last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1133             : 
+    1134        1040 :     last_control_output_.diagnostics.controller = "Se3Controller";
+    1135             : 
+    1136        1040 :     return;
+    1137             :   }
+    1138             : 
+    1139             :   /* mass estimatior //{ */
+    1140             : 
+    1141             :   // --------------------------------------------------------------
+    1142             :   // |                integrate the mass difference               |
+    1143             :   // --------------------------------------------------------------
+    1144             : 
+    1145             :   {
+    1146       49310 :     std::scoped_lock lock(mutex_gains_);
+    1147             : 
+    1148       24655 :     if (tracker_command.use_position_vertical && !rampup_active_) {
+    1149       24640 :       uav_mass_difference_ += gains.km * Ep(2) * dt;
+    1150             :     }
+    1151             : 
+    1152             :     // saturate the mass estimator
+    1153       24655 :     bool uav_mass_saturated = false;
+    1154       24655 :     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       24655 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1158           0 :       uav_mass_difference_ = gains.km_lim;
+    1159           0 :       uav_mass_saturated   = true;
+    1160       24655 :     } 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       24655 :     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       24655 :   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       24655 :   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       24655 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1188             : 
+    1189       24655 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "Se3Controller");
+    1190             : 
+    1191       24655 :   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       24655 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1205             : 
+    1206             :   // --------------------------------------------------------------
+    1207             :   // |               desired orientation + throttle               |
+    1208             :   // --------------------------------------------------------------
+    1209             : 
+    1210             :   // | ------------------- desired orientation ------------------ |
+    1211             : 
+    1212       24655 :   Eigen::Matrix3d Rd;
+    1213             : 
+    1214       24655 :   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       24655 :     Eigen::Vector3d bxd;  // desired heading vector
+    1231             : 
+    1232       24655 :     if (tracker_command.use_heading) {
+    1233       24655 :       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       24655 :     Rd = common::so3transform(f_normed, bxd, drs_params.rotation_type == 1);
+    1240             :   }
+    1241             : 
+    1242             :   // | -------------------- desired throttle -------------------- |
+    1243             : 
+    1244       24655 :   double desired_thrust_force = f.dot(R.col(2));
+    1245       24655 :   double throttle             = 0;
+    1246             : 
+    1247       24655 :   if (tracker_command.use_throttle) {
+    1248             : 
+    1249             :     // the throttle is overriden from the tracker command
+    1250           0 :     throttle = tracker_command.throttle;
+    1251             : 
+    1252       24655 :   } else if (rampup_active_) {
+    1253             : 
+    1254             :     // deactivate the rampup when the times up
+    1255          15 :     if (std::abs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1256             : 
+    1257          14 :       rampup_active_ = false;
+    1258             : 
+    1259          14 :       ROS_INFO("[Se3Controller]: rampup finished");
+    1260             : 
+    1261             :     } else {
+    1262             : 
+    1263           1 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1264             : 
+    1265           1 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1266             : 
+    1267           1 :       rampup_last_time_ = ros::Time::now();
+    1268             : 
+    1269           1 :       throttle = rampup_throttle_;
+    1270             : 
+    1271           1 :       ROS_INFO_THROTTLE(0.1, "[Se3Controller]: ramping up throttle, %.4f", throttle);
+    1272             :     }
+    1273             : 
+    1274             :   } else {
+    1275             : 
+    1276       24640 :     if (desired_thrust_force >= 0) {
+    1277       24640 :       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       24655 :   bool throttle_saturated = false;
+    1286             : 
+    1287       24655 :   if (!std::isfinite(throttle)) {
+    1288             : 
+    1289           0 :     ROS_ERROR("[Se3Controller]: NaN detected in variable 'throttle'!!!");
+    1290           0 :     return;
+    1291             : 
+    1292       24655 :   } 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       24655 :   } 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       24655 :   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       24655 :   Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1321             : 
+    1322             :   {
+    1323       24655 :     Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1324             : 
+    1325       49310 :     geometry_msgs::Vector3Stamped world_accel;
+    1326             : 
+    1327       24655 :     world_accel.header.stamp    = ros::Time::now();
+    1328       24655 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1329       24655 :     world_accel.vector.x        = unbiased_des_acc_world(0);
+    1330       24655 :     world_accel.vector.y        = unbiased_des_acc_world(1);
+    1331       24655 :     world_accel.vector.z        = unbiased_des_acc_world(2);
+    1332             : 
+    1333       73965 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1334             : 
+    1335       24655 :     if (res) {
+    1336       24655 :       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       24655 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1344             : 
+    1345             :   // fill the unbiased desired accelerations
+    1346       24655 :   last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1347             : 
+    1348             :   // | ----------------- fill in the diagnostics ---------------- |
+    1349             : 
+    1350       24655 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1351             : 
+    1352       24655 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1353       24655 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1354       24655 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1355             : 
+    1356       24655 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1357             : 
+    1358       24655 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1359       24655 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1360             : 
+    1361       24655 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1362       24655 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1363             : 
+    1364       24655 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1365       24655 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1366             : 
+    1367       24655 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1368             : 
+    1369       24655 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1370             : 
+    1371             :   // | ------------ construct the attitude reference ------------ |
+    1372             : 
+    1373       24655 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1374             : 
+    1375       24655 :   attitude_cmd.stamp       = ros::Time::now();
+    1376       24655 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1377       24655 :   attitude_cmd.throttle    = throttle;
+    1378             : 
+    1379       24655 :   if (output_modality == common::ATTITUDE) {
+    1380             : 
+    1381        1261 :     last_control_output_.control_output = attitude_cmd;
+    1382             : 
+    1383        1261 :     return;
+    1384             :   }
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                      attitude control                      |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390       23394 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1391             : 
+    1392       23394 :   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       23394 :   } else if (tracker_command.use_heading_rate) {
+    1397             : 
+    1398             :     // to fill in the feed forward yaw rate
+    1399       23394 :     double desired_yaw_rate = 0;
+    1400             : 
+    1401             :     try {
+    1402       23394 :       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       23394 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1409             :   }
+    1410             : 
+    1411             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1412             : 
+    1413       23394 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1414             : 
+    1415       23394 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1416             : 
+    1417       23375 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: using jerk feedforward");
+    1418             : 
+    1419       23375 :     Eigen::Matrix3d I;
+    1420       23375 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1421       23375 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1422       23375 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1423             :   }
+    1424             : 
+    1425             :   // | --------------- run the attitude controller -------------- |
+    1426             : 
+    1427       23394 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1428             : 
+    1429       23394 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq,
+    1430       46788 :                                                           drs_params.pitch_roll_heading_rate_compensation);
+    1431             : 
+    1432       23394 :   if (!attitude_rate_command) {
+    1433           0 :     return;
+    1434             :   }
+    1435             : 
+    1436             :   // | --------- fill in the already known attitude rate -------- |
+    1437             : 
+    1438             :   {
+    1439             :     try {
+    1440       23394 :       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       23394 :   if (output_modality == common::ATTITUDE_RATE) {
+    1449             : 
+    1450       20783 :     last_control_output_.control_output = attitude_rate_command;
+    1451             : 
+    1452       20783 :     return;
+    1453             :   }
+    1454             : 
+    1455             :   // --------------------------------------------------------------
+    1456             :   // |                    Attitude rate control                   |
+    1457             :   // --------------------------------------------------------------
+    1458             : 
+    1459        2611 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1460             : 
+    1461        2611 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1462             : 
+    1463        2611 :   if (!control_group_command) {
+    1464           0 :     return;
+    1465             :   }
+    1466             : 
+    1467        2611 :   if (output_modality == common::CONTROL_GROUP) {
+    1468             : 
+    1469        1308 :     last_control_output_.control_output = control_group_command;
+    1470             : 
+    1471        1308 :     return;
+    1472             :   }
+    1473             : 
+    1474             :   // --------------------------------------------------------------
+    1475             :   // |                        output mixer                        |
+    1476             :   // --------------------------------------------------------------
+    1477             : 
+    1478        2606 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1479             : 
+    1480        1303 :   last_control_output_.control_output = actuator_cmd;
+    1481             : 
+    1482        1303 :   return;
+    1483             : }
+    1484             : 
+    1485             : //}
+    1486             : 
+    1487             : /* positionPassthrough() //{ */
+    1488             : 
+    1489         249 : void Se3Controller::positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1490             : 
+    1491         249 :   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         498 :   mrs_msgs::HwApiPositionCmd cmd;
+    1497             : 
+    1498         249 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1499         249 :   cmd.header.stamp    = ros::Time::now();
+    1500             : 
+    1501         249 :   cmd.position = tracker_command.position;
+    1502         249 :   cmd.heading  = tracker_command.heading;
+    1503             : 
+    1504         249 :   last_control_output_.control_output = cmd;
+    1505             : 
+    1506             :   // fill the unbiased desired accelerations
+    1507         249 :   last_control_output_.desired_unbiased_acceleration = {};
+    1508         249 :   last_control_output_.desired_orientation           = {};
+    1509         249 :   last_control_output_.desired_heading_rate          = {};
+    1510             : 
+    1511             :   // | ----------------- fill in the diagnostics ---------------- |
+    1512             : 
+    1513         249 :   last_control_output_.diagnostics.ramping_up = false;
+    1514             : 
+    1515         249 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1516         249 :   last_control_output_.diagnostics.mass_difference = 0;
+    1517             : 
+    1518         249 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1519             : 
+    1520         249 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1521         249 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1522             : 
+    1523         249 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1524         249 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1525             : 
+    1526         249 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1527         249 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1528             : 
+    1529         249 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1530             : 
+    1531         249 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1532             : }
+    1533             : 
+    1534             : //}
+    1535             : 
+    1536             : /* PIDVelocityOutput() //{ */
+    1537             : 
+    1538        3017 : 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        3017 :   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        3017 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1547        3017 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1548             : 
+    1549        3017 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1550        3017 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1551             : 
+    1552        3017 :   double hdg_ref = tracker_command.heading;
+    1553        3017 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1554             : 
+    1555             :   // | ------------------ velocity feedforward ------------------ |
+    1556             : 
+    1557        3017 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1558             : 
+    1559        3017 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1560        3017 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1561             :   }
+    1562             : 
+    1563             :   // | -------------------------- gains ------------------------- |
+    1564             : 
+    1565        3017 :   Eigen::Vector3d Kp;
+    1566             : 
+    1567             :   {
+    1568        3017 :     std::scoped_lock lock(mutex_gains_);
+    1569             : 
+    1570        3017 :     Kp << gains.kpxy, gains.kpxy, gains.kpz;
+    1571             :   }
+    1572             : 
+    1573             :   // | --------------------- control errors --------------------- |
+    1574             : 
+    1575        3017 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1576             : 
+    1577             :   // | --------------------------- pid -------------------------- |
+    1578             : 
+    1579        3017 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1580        3017 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1581        3017 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1582             : 
+    1583        3017 :   double des_vel_x = position_pid_x_.update(Ep(0), dt);
+    1584        3017 :   double des_vel_y = position_pid_y_.update(Ep(1), dt);
+    1585        3017 :   double des_vel_z = position_pid_z_.update(Ep(2), dt);
+    1586             : 
+    1587             :   // | -------------------- position feedback ------------------- |
+    1588             : 
+    1589        3017 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1590             : 
+    1591        3017 :   if (control_output == common::VELOCITY_HDG) {
+    1592             : 
+    1593             :     // | --------------------- fill the output -------------------- |
+    1594             : 
+    1595        2232 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1596             : 
+    1597        1116 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1598        1116 :     cmd.header.stamp    = ros::Time::now();
+    1599             : 
+    1600        1116 :     cmd.velocity.x = des_vel(0);
+    1601        1116 :     cmd.velocity.y = des_vel(1);
+    1602        1116 :     cmd.velocity.z = des_vel(2);
+    1603             : 
+    1604        1116 :     cmd.heading = tracker_command.heading;
+    1605             : 
+    1606        1116 :     last_control_output_.control_output = cmd;
+    1607             : 
+    1608        1901 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1609             : 
+    1610        1901 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1611             : 
+    1612        1901 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1613             : 
+    1614        1901 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1615             : 
+    1616             :     // | --------------------------- ff --------------------------- |
+    1617             : 
+    1618        1901 :     double des_hdg_ff = 0;
+    1619             : 
+    1620        1901 :     if (tracker_command.use_heading_rate) {
+    1621        1901 :       des_hdg_ff = tracker_command.heading_rate;
+    1622             :     }
+    1623             : 
+    1624             :     // | --------------------- fill the output -------------------- |
+    1625             : 
+    1626        3802 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1627             : 
+    1628        1901 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1629        1901 :     cmd.header.stamp    = ros::Time::now();
+    1630             : 
+    1631        1901 :     cmd.velocity.x = des_vel(0);
+    1632        1901 :     cmd.velocity.y = des_vel(1);
+    1633        1901 :     cmd.velocity.z = des_vel(2);
+    1634             : 
+    1635        1901 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1636             : 
+    1637        1901 :     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        3017 :   last_control_output_.desired_unbiased_acceleration = {};
+    1646        3017 :   last_control_output_.desired_orientation           = {};
+    1647        3017 :   last_control_output_.desired_heading_rate          = {};
+    1648             : 
+    1649             :   // | ----------------- fill in the diagnostics ---------------- |
+    1650             : 
+    1651        3017 :   last_control_output_.diagnostics.ramping_up = false;
+    1652             : 
+    1653        3017 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1654        3017 :   last_control_output_.diagnostics.mass_difference = 0;
+    1655             : 
+    1656        3017 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1657             : 
+    1658        3017 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1659        3017 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1660             : 
+    1661        3017 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1662        3017 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1663             : 
+    1664        3017 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1665        3017 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1666             : 
+    1667        3017 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1668             : 
+    1669        3017 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1670             : }
+    1671             : 
+    1672             : //}
+    1673             : 
+    1674             : // --------------------------------------------------------------
+    1675             : // |                          callbacks                         |
+    1676             : 
+    1677             : 
+    1678             : /* //{ callbackDrs() */
+    1679             : 
+    1680         215 : void Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, [[maybe_unused]] uint32_t level) {
+    1681             : 
+    1682         215 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1683             : 
+    1684         215 :   ROS_INFO("[Se3Controller]: DRS updated gains");
+    1685         215 : }
+    1686             : 
+    1687             : //}
+    1688             : 
+    1689             : // --------------------------------------------------------------
+    1690             : // |                           timers                           |
+    1691             : // --------------------------------------------------------------
+    1692             : 
+    1693             : /* timerGains() //{ */
+    1694             : 
+    1695        4686 : void Se3Controller::timerGains(const ros::TimerEvent& event) {
+    1696             : 
+    1697        9372 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1698        9372 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1699             : 
+    1700        4686 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1701        4686 :   auto gains      = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1702             : 
+    1703             :   // When muting the gains, we want to bypass the filter,
+    1704             :   // so it happens immediately.
+    1705        4686 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    1706        4686 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    1707             : 
+    1708        4686 :   mute_gains_ = false;
+    1709             : 
+    1710        4686 :   double dt = (event.current_real - event.last_real).toSec();
+    1711             : 
+    1712        4686 :   if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
+    1713          27 :     return;
+    1714             :   }
+    1715             : 
+    1716        4659 :   bool updated = false;
+    1717             : 
+    1718        4659 :   gains.kpxy          = calculateGainChange(dt, gains.kpxy, drs_params.kpxy * gain_coeff, bypass_filter, "kpxy", updated);
+    1719        4659 :   gains.kvxy          = calculateGainChange(dt, gains.kvxy, drs_params.kvxy * gain_coeff, bypass_filter, "kvxy", updated);
+    1720        4659 :   gains.kaxy          = calculateGainChange(dt, gains.kaxy, drs_params.kaxy * gain_coeff, bypass_filter, "kaxy", updated);
+    1721        4659 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    1722        4659 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    1723        4659 :   gains.kpz           = calculateGainChange(dt, gains.kpz, drs_params.kpz * gain_coeff, bypass_filter, "kpz", updated);
+    1724        4659 :   gains.kvz           = calculateGainChange(dt, gains.kvz, drs_params.kvz * gain_coeff, bypass_filter, "kvz", updated);
+    1725        4659 :   gains.kaz           = calculateGainChange(dt, gains.kaz, drs_params.kaz * gain_coeff, bypass_filter, "kaz", updated);
+    1726        4659 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    1727        4659 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    1728        4659 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    1729             : 
+    1730             :   // do not apply muting on these gains
+    1731        4659 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    1732        4659 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    1733        4659 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    1734             : 
+    1735        4659 :   mrs_lib::set_mutexed(mutex_gains_, gains, gains_);
+    1736             : 
+    1737             :   // set the gains back to dynamic reconfigure
+    1738             :   // and only do it when some filtering occurs
+    1739        4659 :   if (updated) {
+    1740             : 
+    1741         540 :     drs_params.kpxy          = gains.kpxy;
+    1742         540 :     drs_params.kvxy          = gains.kvxy;
+    1743         540 :     drs_params.kaxy          = gains.kaxy;
+    1744         540 :     drs_params.kiwxy         = gains.kiwxy;
+    1745         540 :     drs_params.kibxy         = gains.kibxy;
+    1746         540 :     drs_params.kpz           = gains.kpz;
+    1747         540 :     drs_params.kvz           = gains.kvz;
+    1748         540 :     drs_params.kaz           = gains.kaz;
+    1749         540 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    1750         540 :     drs_params.kq_yaw        = gains.kq_yaw;
+    1751         540 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    1752         540 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    1753         540 :     drs_params.km            = gains.km;
+    1754         540 :     drs_params.km_lim        = gains.km_lim;
+    1755             : 
+    1756         540 :     drs_->updateConfig(drs_params);
+    1757             : 
+    1758         540 :     ROS_INFO_THROTTLE(10.0, "[Se3Controller]: gains have been updated");
+    1759             :   }
+    1760             : }
+    1761             : 
+    1762             : //}
+    1763             : 
+    1764             : // --------------------------------------------------------------
+    1765             : // |                       other routines                       |
+    1766             : // --------------------------------------------------------------
+    1767             : 
+    1768             : /* calculateGainChange() //{ */
+    1769             : 
+    1770       65226 : double Se3Controller::calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name,
+    1771             :                                           bool& updated) {
+    1772             : 
+    1773       65226 :   double change = desired_value - current_value;
+    1774             : 
+    1775       65226 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    1776       65226 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    1777             : 
+    1778       65226 :   if (!bypass_rate) {
+    1779             : 
+    1780             :     // if current value is near 0...
+    1781             :     double change_in_perc;
+    1782             :     double saturated_change;
+    1783             : 
+    1784       65226 :     if (std::abs(current_value) < 1e-6) {
+    1785           0 :       change *= gains_filter_max_change;
+    1786             :     } else {
+    1787             : 
+    1788       65226 :       saturated_change = change;
+    1789             : 
+    1790       65226 :       change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;
+    1791             : 
+    1792       65226 :       if (change_in_perc > gains_filter_max_change) {
+    1793        1269 :         saturated_change = current_value * gains_filter_max_change;
+    1794       63957 :       } else if (change_in_perc < -gains_filter_max_change) {
+    1795         162 :         saturated_change = current_value * -gains_filter_max_change;
+    1796             :       }
+    1797             : 
+    1798       65226 :       if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
+    1799          28 :         change *= gains_filter_min_change;
+    1800             :       } else {
+    1801       65198 :         change = saturated_change;
+    1802             :       }
+    1803             :     }
+    1804             :   }
+    1805             : 
+    1806       65226 :   if (std::abs(change) > 1e-3) {
+    1807        1593 :     ROS_DEBUG("[Se3Controller]: changing gain '%s' from %.2f to %.2f", name.c_str(), current_value, desired_value);
+    1808        1593 :     updated = true;
+    1809             :   }
+    1810             : 
+    1811       65226 :   return current_value + change;
+    1812             : }
+    1813             : 
+    1814             : //}
+    1815             : 
+    1816             : /* getHeadingSafely() //{ */
+    1817             : 
+    1818       28712 : double Se3Controller::getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1819             : 
+    1820             :   try {
+    1821       28712 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1822             :   }
+    1823           0 :   catch (...) {
+    1824             :   }
+    1825             : 
+    1826             :   try {
+    1827           0 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+    1828             :   }
+    1829           0 :   catch (...) {
+    1830             :   }
+    1831             : 
+    1832           0 :   if (tracker_command.use_heading) {
+    1833           0 :     return tracker_command.heading;
+    1834             :   }
+    1835             : 
+    1836           0 :   return 0;
+    1837             : }
+    1838             : 
+    1839             : //}
+    1840             : 
+    1841             : }  // namespace se3_controller
+    1842             : 
+    1843             : }  // namespace mrs_uav_controllers
+    1844             : 
+    1845             : #include <pluginlib/class_list_macros.h>
+    1846         107 : PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::se3_controller::Se3Controller, mrs_uav_managers::Controller)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html b/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html new file mode 100644 index 0000000000..3721a40cc3 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html @@ -0,0 +1,482 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/se3_controller.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.png b/mrs_uav_controllers/src/se3_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..7534d79b912483db0b6a081f75d0068fd5bfdfc7 GIT binary patch literal 5854 zcmV<479r`0P)vX+d=hvT0 zukyloAidU5CcrAFwdz?5*r(nLCg$3OC7FTr%-dc453j$PQ)K_hyT0vN`aC~g# zpvQRb=N!g;u=+A-0gIq5fO8+ZuBh$;&hBhPRTlvVR6QMXHub~oX{iJ1t8_!*c$^MC z?f>=#bfD8UtUwrW4uG@&K1WIbef5kSUDK+7FlLYhFklp_o*~s-y{7;jsF%{|iUBVX zaFU1)#1KHHZ_>0k`>F+wV~irZ8H^L#fms_xz_~h3t!2Q}JOyDsM7syZFh$LC;^?U< zxIE2b$WUKeZx}5Dp)M6uBZKh7id)dI#Pkv(q#u!|Bn39AF|G+eg_*#F5iINx;~Bvu zLP!a3K+b5cC)(#Q&J-iVQ$s`bZA#2&YdVK9G8PHI5t$5DV0nLItP3YG% z?$d^;k4QiTaDV7FVI*RvSpRfNm4;oAe=(!a>{yuTk0rnhtpa_CznIDAdgHHAdITM! zjr&-7Ch%`N!_g!q%*;(oToED0I0XqhX_M0GxQbELlh9gw)dPA4mY^+3fXwgOdFhFn zV!e}7Uc}`UH+dr=#n!5^!7rS@uvWJs_GKpB~A1| z&28r?5$IMl5F;5g#1c3$W@gYvR%sx^$~8a$#J@N#mWXOUtUxo)46fA6QL#woh6OL_ z2=X*(2Xmo~;%_s+^IEfKcx?O}gJ!DJB3$%iUz;2&m8v6eisEpEmTrl}q|;R8QHv4DRV5fVYpPZk)YslWds_L> zxuN<4{%dW7z5PZ&5w@?t{=hiGXJ0HSD3w+wpeUvG%w~+1Hijyj>BQxBTW`38&)e%y zDwhQp`l|$}F3W9u)v;6yt-1gh8nMKtR{*yN$Tq}U^@`cj0b`L4BgQa?&(~<{Oi@?3 zw#?z6jgmmDob+g^rg&0MSupZVTPfiE{=<~V{@>&0`TFC^-2uMdKkwyoxz`{9puWIm zcn)0$RDv^ISS$uWLtne$7Y5ovb?l&1iMTtxYI)_7Un#cdGwF8)-CU zBFbh<&xo#x8#MZusrj(+c4N=Ih%@d-n0wx6pHF5CaqQWE&nG-xxWNfmx$w*`anFNe zJ3k-(pZ6|tQIo2zO3ay` zzWz`?7TTyKW|%L9^OhmLwYITGbdOUN{3H-Q9CGqYfE;Xd7ly4sXW7F`V(PzFW1HJJ z1y!L}7fQpwWnAxsmqwQjqcqU zu7Eow_GmF?(dHv$?;_42dcXg7(S?v*c>nSGg#vI~ct38kjYkNyC*%`fDqotMfVDBx z(uGg>@Q7WyXAhXV@F%2g94Y$0-Gx)!Js$t{CxCUy&_FJj@QnQw8+lmWI?`;UVvt@_?$UNBH&tdqi?_Vv;lf5xIR5G2#TRB4&U4QN$DkmnQ`i z^I1J3^o^F7+s6=Nm;g9pAmICnDX28Mp4q;{e2I~W(HMM&s=IrTh^io0{ij^<-Okd^ zR!8^&NasnXG@DV`gt3r3)0U0X$49E~!l6ex^zoe|Gc%P3eKOdtIid$Np%S10qeH_q z^$AQ7Jw|WjdcTBc>NLkw)9gm>oo!ZbBenAJ=SVd0$cup3MguKqtlDwCFQ8~88ij(B z2vi;>!F}jbIrJFssU1v*G-G^JT|*W?W!3vR_4XsoiepA{KRn~6o8$kV!0`GWA2p!X zMWTc=5HtLYp$==3ups@=q1iB-m1V+)5l^y0SLp%&R9|F8DRjZ=_ z)D=%l=-%Jj$G+wt&M54Gn-s7OV?-=%s;X_2K!A}#0npOMLhL9C)Soi}xWb+4b_oZ& z!8c=1b@M89FYA@*z>PeuH+BmYo-k5ht4mD0VB?Qz*JcV;vVduzdBBD4pN&v<35U4~ zllL5?t+cJFmn=0T1Y$o}MJ>5kKuGY09A6BT+&M+7Cm+E69YAdXJ9kjeWDFkwsO@WZ zqfLIKOzU0%D2_}Uvm_}wQlSIXrMkFH1226~qr_G z7R}mvsRdkkCT9T=36OqXZMxYMH5kQFEy95g;ai)^HP7Loof5w#s;NA;XrwW9HXVr6 zq>;u$7;8Hwk*{DagaOsW@UvO2&$wJy#UaoGa$Wxdd+zLs#E(1luk0@CH}er#71Nd8 z(06VZ8*3UiFkIV(p@vTeR2IFq2Xfdcj~lCQvOa3b#ah7syY*3|`kmItNvglnW8`Bx z4Lx399xvSsTHyYrz;JfG{>R0PGd~WhS9nmpzyKT78uR0~1{Xs%q}q)>AQ*5U4N9Ja zO1+4&39vi?4A@k4sAi7MH@U3_KxG+lP~;^Y{W3*+)rzU@*vK?wzb7#<@Hzr6W$8j5CiU!@N4*9tJ#`z@@yN02CGw{i?v3Qo<-SV|4nM zBLqCfWq6Q_L=!JevGcITOz8W;vGb6V+9%PbC1W7ej@HA0RG%>ISz&LGN3;(vcUTzK zYdA}loYzLX5XIwJ=a@f@jJuTMWs_h%d^c0P-_2!-vge$Qv*PAzXn^iH^T$21U8vbL z7?-$elBT>C9*>T}$DUkppj=vunbkI63^l~WT-p-qBNcb@^Hp<(k5o0qtFL;f6}PW# z;u(jejSukf@Phmd$0fW~aKUI6&(Zh_?pJWn2<`y&dPKTnjGy$$20W$Cz~%Te)tU2= ziXe?()eLnkA?J6q04l4FC+UK_UDA1ih>N|&Y~fuWr#v6%Fp`qzgCtdp#~T5_;LxaU zq3;H0pt|PzFnT_8u8#vF%fvH)D%Z!cr-ENKk?Z5whoqXD2S|AT!L@wHOa;(D^>@sC z>X`9{c+sza#t?T8IeXd-arZ#kGuECdf>A@fFlPRoJyYDMje1Smyw_~chzE%2s~&65 zY#Z^vFvKJ?CO$mk#@)St-YvMG`T2bJ72L1j62Xm@_`hX{BR2))4LJ+3(b9y*PEv}P&kr-FhcU5XHkaKr?HSupZzH#P2c({DZGwv=m6f$-j3FZiN*G@?88-sp z{^nN!4OCb2$Sl{kE-j8B7?YxpmIY~ucV?zTl`-fFb3O7f+9DO;yoBUQZ9XqSXk%t` zw&Y1soY?(RU=PlO6x&EI0PoPo&^H}1;w*g&V5h3L2Ee~eHNt!zW17m_+NhYO2Bi%4 z3|?QF;@2>IodEvT_;qpXPB6dldhOP@ea6Xr@q-ji)FUPKU`5PEbsmdD&a28fn!J}Q z`nUYqirJN@QTgABRrAhATJ^nf6OCLo_4lwk9dg?yh3OsV8~>AhKHY@c=y$Vfe!O^l*b$<;fkTf9}c6BOg$Ur@u*rM zZ9>tYXo5|Za7G-u+XYVm*bF~$*hcN_kjCL!7_>I`LtKR-#Zx*X$4`Xe zCO@$RDYV@Uh;i@-o*!DuQ#BOswN|>KPXxNVl$dJuwH%~pxI^)+IY`}4O^5zoC!78+ z5%@DUmwZQeeu@J9=q{=`z3J5deWoMeW17?t)O<+C4$|xik=rc`qK|Sv;5%q%G(DbG zkaDLC=xYz?X%Zu2!Bf0E z;vt7W#_Ju`RWET)sa^VO#~3FGHof>m5(a3_N-WJ()15#FeG^7tB=dkKj8blPJI2)0 z>A@$LxlbcJaU)h&Xq|1fTN^9UJx6uRS_qc9XSfC0nO$!_fFZNkPb=&i7is%CQ@nRi zhmpIZ8zylcFYm#2)oRVgbp|;f@)OhYB~DDe zu_DH$zKkV;qbv zxvm1u8%TXB0@AIRQX6T8R);o*+B;$-=cpP0J5{|k0RCmF5$5w4b7x?~a0RFR$Dn(M z;VP%Qw@|$-;@}mBuzQJD|ATfO?F4KE7*nJI42=;}r{k`(2Pj@M>I4 zGt*p!OSMi#AX#;(#i5b!QHRD#t3O*etOI!1Xstne;hk0}6>7|CsHI29hIbK+7W&P@SJgP#J0mN_nJ zqI&?mRd8i5={xhJLKmV0nv^=&VqfcjVP6%HEKngCvMRhiG=wdHl$P5vxY*+WGQsSk z3?HQt<6lySQ@5o47^w_b1Ej|!*K>Y6m5=+O)82O|luMgkc)4ay)bVy<5>Y;({EQoi zzY@lVQPVjWWe*y_0P~=T&mUXZJMGG67xsKzXBT$t{0XP+sLQ{m0XS+{yDZQY zt+)lo;Sf|3T&skW(+D%nc!wAdXid_-F(-$yN!kZsMsya6Y+DP#-jFUE=x$QJm$8kO z+tbB3%8CF7-}-$ecYc4yM#MLt%CRM}kS>fJ8u5vOJ)yoi(=)3dwWs$X5$TC4X=jZz z@6B$2QDW@VUcS$sW)}1ZZ0eafLCzg|hFNFU@O~$tWq!nGq2z#Ss_CE!d&-*0X0m4m zV$@VCZF>|i?6n0d@Oxz{91Lh{W99%f#rZ&NX%Ea`_v>Zw7GS0kDInJ!#_mPJ1;#Ya0W~aDjXW zjEM0Jpl+L{K^}!|gFj(I=kexoT>1CTe2TAUST0}Ba2v+|r+fD68D2idGt3qt4d8;l z+e`oKDA>j+XnB1w@?mLGY+Ld;k+?+RuUBsfCl!s+1AgsenMsV<2ISO zjb|x$o#Ug6Q`Cu3Qyn?Jf0P|6BLQy!kasm%0ha_Q-55Ix&`AzB^+yEUmK2h&U<9#y zENp-~bjKw$H$4P%Zfm!<04!G%*L&eLG@Cb3mq-{#n9VmRh9btl<-P9ZwY#zF^@^W1o297?qAhgQ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..97fd2504ed --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..88a4e9b947 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..2439877494 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..c1effef269 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..45ee068a30 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..a9437db289 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1356
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)123712
+
+
+ + + +
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..db3df8c37f --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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*)123712
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1356
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
+
+
+ + + +
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..c735a4b080 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html @@ -0,0 +1,146 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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      123712 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52      123712 :     void operator()(const T& msg) {
+      53      123712 :       obj_->publish(msg);
+      54      123712 :     }
+      55             :   };
+      56             : };
+      57             : 
+      58             : }  // namespace control_manager
+      59             : 
+      60             : }  // namespace mrs_uav_managers
+      61             : 
+      62             : #endif  //  CONTROL_MANAGER_OUTPUT_PUBLISHER
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html new file mode 100644 index 0000000000..09b263e5e1 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html @@ -0,0 +1,36 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fdd688bb370f7663153724647927eaa459667bca GIT binary patch literal 332 zcmeAS@N?(olHy`uVBq!ia0vp^0YGfW!VDz+4mNH9QW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!CRg#jv*eMTc_R@JgmUO63D)Y^##MP z_qxsoW@77=HNHQc`DTrZj!g2}RPCgyMn{X24zmu;3tZ^#7E%+I9$1z2-~Hw84U=k2 z4|j*$Z0-BVwQ#LZPe@1*@1FIJ-4-|f*Lgnua4f;i^kDBLO$82VM}aqiJexoA?e=!; zP1$C<>VaG4@4cUR8vfjSmptKVZex36aP6U&;q4|#;dgdw2qk^7v)`$HtkLlNGdUR# zYu`;D4^40T_+7%T&EUnw&}9A@Oy}y>9psSfzqj9wQ#d|**7eZ4D=rjGy76rCwIcb5 X-I2 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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&)86
mrs_uav_managers::AglEstimator::~AglEstimator().286
+
+
+ + + +
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..46cb3d19ab --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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&)86
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::~AglEstimator().286
+
+
+ + + +
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..432248c584 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html @@ -0,0 +1,157 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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          86 :   AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      55          86 :       : Estimator(agl::type, name, frame_id), package_name_(package_name) {
+      56          86 :   }
+      57             : 
+      58          86 :   virtual ~AglEstimator(void) {
+      59          86 :   }
+      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..bdb5f888a9 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html @@ -0,0 +1,188 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_manager - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)535
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
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&)905
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
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&)1168
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
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&)1356
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1356
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&)2786
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)3345
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&)3997
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&)4010
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5388
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5403
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&)8372
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13907
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)14116
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&)86471
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103346
+
+
+ + + +
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..0a4dcf0c23 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html @@ -0,0 +1,188 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_manager - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::HwApiValidateVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3997
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&)8372
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&)535
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&)1356
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&)86471
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&)4010
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&)905
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&)2786
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&)1168
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)14116
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)0
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5388
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13907
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1356
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103346
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5403
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)3345
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
+
+
+ + + +
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..6884601b0e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html @@ -0,0 +1,380 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_manager - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef CONTROL_MANAGER_COMMON_H
+       2             : #define CONTROL_MANAGER_COMMON_H
+       3             : 
+       4             : #include <ros/ros.h>
+       5             : 
+       6             : #include <vector>
+       7             : #include <string>
+       8             : #include <optional>
+       9             : #include <variant>
+      10             : 
+      11             : #include <mrs_lib/attitude_converter.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : 
+      14             : #include <mrs_uav_managers/control_manager/common_handlers.h>
+      15             : #include <mrs_uav_managers/controller.h>
+      16             : 
+      17             : #include <mrs_msgs/TrackerCommand.h>
+      18             : #include <mrs_msgs/UavState.h>
+      19             : #include <mrs_msgs/VelocityReference.h>
+      20             : 
+      21             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      22             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      23             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      24             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      25             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      26             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      27             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      28             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      29             : #include <mrs_msgs/HwApiPositionCmd.h>
+      30             : 
+      31             : #include <mrs_msgs/HwApiCapabilities.h>
+      32             : 
+      33             : #include <nav_msgs/Odometry.h>
+      34             : 
+      35             : namespace mrs_uav_managers
+      36             : {
+      37             : 
+      38             : namespace control_manager
+      39             : {
+      40             : 
+      41             : enum CONTROL_OUTPUT
+      42             : {
+      43             :   ACTUATORS_CMD,
+      44             :   CONTROL_GROUP,
+      45             :   ATTITUDE_RATE,
+      46             :   ATTITUDE,
+      47             :   ACCELERATION_HDG_RATE,
+      48             :   ACCELERATION_HDG,
+      49             :   VELOCITY_HDG_RATE,
+      50             :   VELOCITY_HDG,
+      51             :   POSITION
+      52             : };
+      53             : 
+      54             : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs);
+      55             : 
+      56             : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs);
+      57             : 
+      58             : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec);
+      59             : 
+      60             : // checks for invalid values in the result from trackers
+      61             : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name);
+      62             : 
+      63             : // checks for invalid messages in/out
+      64             : bool validateOdometry(const nav_msgs::Odometry& msg, const std::string& node_name, const std::string& var_name);
+      65             : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_nam);
+      66             : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name);
+      67             : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name);
+      68             : 
+      69             : std::optional<DetailedModelParams_t> loadDetailedUavModelParams(ros::NodeHandle& nh, const std::string& node_name, const std::string& platform_config,
+      70             :                                                                 const std::string& custom_config);
+      71             : 
+      72             : // translates the channel values to desired range
+      73             : double RCChannelToRange(double rc_value, double range, double deadband);
+      74             : 
+      75             : /* throttle extraction //{ */
+      76             : 
+      77             : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output);
+      78             : 
+      79             : struct HwApiCmdExtractThrottleVisitor
+      80             : {
+      81        5388 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83        5388 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87        5388 :     double throttle = 0;
+      88             : 
+      89       26940 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90       21552 :       throttle += msg.motors.at(i);
+      91             :     };
+      92             : 
+      93        5388 :     throttle /= msg.motors.size();
+      94             : 
+      95        5388 :     return throttle;
+      96             :   }
+      97        5403 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98        5403 :     return msg.throttle;
+      99             :   }
+     100       13907 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101       13907 :     return msg.throttle;
+     102             :   }
+     103      103346 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104      103346 :     return msg.throttle;
+     105             :   }
+     106        1168 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107        1168 :     return std::nullopt;
+     108             :   }
+     109         905 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110         905 :     return std::nullopt;
+     111             :   }
+     112        3345 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113        3345 :     return std::nullopt;
+     114             :   }
+     115        1356 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116        1356 :     return std::nullopt;
+     117             :   }
+     118         535 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119         535 :     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        3997 :   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        3997 :     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        3997 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153        4010 :   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        4010 :     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        4010 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        8372 :   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        8372 :     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        8372 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173       86471 :   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       86471 :     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       86471 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183        1168 :   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        1168 :     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        1168 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193         905 :   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         905 :     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         905 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203        2786 :   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        2786 :     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        2786 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213        1356 :   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        1356 :     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        1356 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223         535 :   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         535 :     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         535 :     return validateHwApiPositionCmd(msg, node_name, var_name);
+     232             :   }
+     233             : };
+     234             : 
+     235             : //}
+     236             : 
+     237             : /* control output initialization //{ */
+     238             : 
+     239             : Controller::HwApiOutputVariant initializeDefaultOutput(const ControlOutputModalities_t& possible_outputs, const mrs_msgs::UavState& uav_state,
+     240             :                                                        const double& min_throttle, const double& n_motors);
+     241             : 
+     242             : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors);
+     243             : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle);
+     244             : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle);
+     245             : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle);
+     246             : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state);
+     247             : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state);
+     248             : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state);
+     249             : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state);
+     250             : void initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state);
+     251             : 
+     252             : struct HwApiInitializeVisitor
+     253             : {
+     254           0 :   void operator()(mrs_msgs::HwApiActuatorCmd& msg, [[maybe_unused]] const mrs_msgs::UavState& uav_state, const double& min_throttle, const double& n_motors) {
+     255           0 :     initializeHwApiCmd(msg, min_throttle, n_motors);
+     256           0 :   }
+     257           0 :   void operator()(mrs_msgs::HwApiControlGroupCmd& msg, [[maybe_unused]] const mrs_msgs::UavState& uav_state, const double& min_throttle,
+     258             :                   [[maybe_unused]] const double& n_motors) {
+     259           0 :     initializeHwApiCmd(msg, min_throttle);
+     260           0 :   }
+     261           0 :   void operator()(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle, [[maybe_unused]] const double& n_motors) {
+     262           0 :     initializeHwApiCmd(msg, uav_state, min_throttle);
+     263           0 :   }
+     264       14116 :   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       14116 :     initializeHwApiCmd(msg, min_throttle);
+     267       14116 :   }
+     268           0 :   void operator()(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     269             :                   [[maybe_unused]] const double& n_motors) {
+     270           0 :     initializeHwApiCmd(msg, uav_state);
+     271           0 :   }
+     272           0 :   void operator()(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     273             :                   [[maybe_unused]] const double& n_motors) {
+     274           0 :     initializeHwApiCmd(msg, uav_state);
+     275           0 :   }
+     276           0 :   void operator()(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     277             :                   [[maybe_unused]] const double& n_motors) {
+     278           0 :     initializeHwApiCmd(msg, uav_state);
+     279           0 :   }
+     280           0 :   void operator()(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     281             :                   [[maybe_unused]] const double& n_motors) {
+     282           0 :     initializeHwApiCmd(msg, uav_state);
+     283           0 :   }
+     284           0 :   void operator()(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     285             :                   [[maybe_unused]] const double& n_motors) {
+     286           0 :     initializeHwApiCmd(msg, uav_state);
+     287           0 :   }
+     288             : };
+     289             : 
+     290             : //}
+     291             : 
+     292             : }  // namespace control_manager
+     293             : 
+     294             : }  // namespace mrs_uav_managers
+     295             : 
+     296             : #endif  // CONTROL_MANAGER_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html new file mode 100644 index 0000000000..29005b23a1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html @@ -0,0 +1,94 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ae8715691990bae086ba95dad2bda32a5157e3ef GIT binary patch literal 936 zcmV;Z16TZsP)8$oMOfwoEbkk%?&H<7fC8styUCF6PPJe1% z_gmVaWp93Y9Y)Q18fo;)0WYrzox(pZh`^9(mym_9VR1Jiyj^GC-yRcl(E zrJ`n-U9PS(ceN52Gh?3!fGh&+Dhr}U1`QK}q8hFPw$%Z)3`b!z^Gvx}YnmHvtVc!1 zh;Ey*F%=0v$?i{!m3co93ly=;^E3?;QQknOU|z)MVH~)w>$YvLP05;;H&AA710Y$Z z$%Hx2%Rw;osAr|`9kKxNFCx~cM%9jXVthUtX{O7f29{I$i=sB^as*6eK?FR^N1c(t zbl!MR({?IcZ~D@x_cM6mG5wfsFnvNZ-2F$FT3cDqCLmm{;sy!K9)t$R60vXMiQ$o=6n{0hS!s z0a&2AC!%>x7T6XVn`rGYZK0Bv?hd6omZn*h=923l54yuM`$r(cjcP@GOfnO2a|tsz zH#BBm_hSa*rUNtIm@|>G0%o2$bJsa@LuO8zGrMZja9+d=#tw~{?T*aYQPmXnV@9|i z0+%rpSF~SdCgA0#nThKel^OYOm;t<8#tha2{4O&QFHdiF4`$kWv)_B))8ma#{zZX1 z&FL_2e@egfN`8Ul_5t-~Q|4185843;w;&Hpk0aO}&I92lat|PYl>}3g6mZWs4J0Su zYPZdxNOx80vNE{R1C{{wH!LZ%in1n5$HoWyh%0^yp-?Zcn#De1+kve(E)2uGvC9<$66nK z4|Q93FCWqA`ZXVO-zRuL`l`^qo#Bj|b?RuLsnpo#8}@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
<unnamed>55.2 %53 / 9670.4 %19 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..e0f14dbd29 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
<unnamed>55.2 %53 / 9670.4 %19 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html new file mode 100644 index 0000000000..2b8421d98d --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
<unnamed>55.2 %53 / 9670.4 %19 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html new file mode 100644 index 0000000000..43a3cff840 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html new file mode 100644 index 0000000000..75199585e8 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html new file mode 100644 index 0000000000..8d38b6347d --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-06-08 22:13:12Functions:192770.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html new file mode 100644 index 0000000000..73fa4fd39a --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-06-08 22:13:12Functions: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().2535
+
+
+ + + +
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..4e8107fe29 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-06-08 22:13:12Functions: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().2535
+
+
+ + + +
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..3c873295a1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-06-08 22:13:12Functions: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         535 :   virtual ~Controller() = default;
+     144             : };
+     145             : 
+     146             : }  // namespace mrs_uav_managers
+     147             : 
+     148             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html new file mode 100644 index 0000000000..7ccc3a2839 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3aa437cffc6a4bac8d6472ee5f8996438a7ffd35 GIT binary patch literal 681 zcmV;a0#^NrP)gyv~hF-^&dW?vH8lQFyr0yFtbG*21IvQ>PFm5wzv~q9s(U*D?$PkPb*mg9K z4;`1wwTa!aQn#n9$1^8R4EK&++_~G$Y7^}l1fj>V&qK*=9V{9pxp(yE8zD8n&L7kKa*6i+*$L2+n!s;Qt zWF?Y#<*tX-BVbX5&}fQ9abk54F_QX+6k<>0_iOxZEXoWOfTDC|i(szm@qstVFc_eX z((J0{GcAMJAVebuH?H!-vd*kO-exr{K^E;Rr%15l!zD`*xd6!R$#h<%{YC~}%o`2Y zF_4q^ZtTtxf79?l#N9r&5hJPP#F|~*gUnc*v;!8oqQ#i0K;)WWsvz1Pkm84qvG(+D zVkB92j8nwb(^YqYUagSMLXiJv9Z|i>lu*k8DdmHzvH*go2mfisL)0D + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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&)621
mrs_uav_managers::Estimator::~Estimator().2621
+
+
+ + + +
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..9140f93b7a --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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&)621
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::~Estimator().2621
+
+
+ + + +
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..c1ea5ec26b --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html @@ -0,0 +1,195 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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         621 :   Estimator(const std::string &type, const std::string &name, const std::string &frame_id) : type_(type), name_(name), frame_id_(frame_id) {
+      64         621 :   }
+      65             : 
+      66         621 :   virtual ~Estimator(void) {
+      67         621 :   }
+      68             : 
+      69             : public:
+      70             :   // virtual methods
+      71             :   virtual void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) = 0;
+      72             :   virtual bool start(void)                                                                                                                = 0;
+      73             :   virtual bool pause(void)                                                                                                                = 0;
+      74             :   virtual bool reset(void)                                                                                                                = 0;
+      75             : 
+      76             :   // implemented methods
+      77             :   // access methods
+      78             :   std::string getName(void) const;
+      79             :   std::string getPrintName(void) const;
+      80             :   std::string getType(void) const;
+      81             :   std::string getFrameId(void) const;
+      82             :   double      getMaxFlightZ(void) const;
+      83             :   std::string getSmStateString(const SMStates_t &state) const;
+      84             :   std::string getCurrentSmStateString(void) const;
+      85             :   SMStates_t  getCurrentSmState() const;
+      86             : 
+      87             :   void setCurrentSmState(const SMStates_t &new_state);
+      88             : 
+      89             :   bool isMitigatingJump();
+      90             : 
+      91             :   // state machine methods
+      92             :   bool changeState(SMStates_t new_state);
+      93             :   bool isInState(const SMStates_t &state_in) const;
+      94             :   bool isInitialized() const;
+      95             :   bool isReady() const;
+      96             :   bool isStarted() const;
+      97             :   bool isRunning() const;
+      98             :   bool isStopped() const;
+      99             :   bool isError() const;
+     100             : 
+     101             :   void publishDiagnostics() const;
+     102             : 
+     103             :   tf2::Vector3          getAccGlobal(const sensor_msgs::Imu::ConstPtr &input_msg, const double hdg);
+     104             :   tf2::Vector3          getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr &input_msg, const double hdg);
+     105             :   tf2::Vector3          getAccGlobal(const geometry_msgs::Vector3Stamped &acc_stamped, const double hdg);
+     106             :   std::optional<double> getHeadingRate(const nav_msgs::OdometryConstPtr &odom_msg);
+     107             : };
+     108             : 
+     109             : }  // namespace mrs_uav_managers
+     110             : 
+     111             : #endif  // MRS_UAV_MANAGERS_ESTIMATION_MANAGER_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html new file mode 100644 index 0000000000..1a1e94470f --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html @@ -0,0 +1,48 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)5
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10
mrs_uav_managers::estimation_manager::Support::isStringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)333
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)428
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2451
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)169146
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)314052
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)358940
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)368195
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)369216
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)394056
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)741558
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)752071
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)1111688
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1224119
+
+
+ + + +
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..cb4c09a15c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-06-08 22:13:12Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)358940
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)5
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)752071
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)1111688
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)428
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2451
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)369216
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)169146
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)741558
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)314052
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)368195
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&)333
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)394056
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1224119
+
+
+ + + +
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..ab098ce8a9 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html @@ -0,0 +1,405 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-06-08 22:13:12Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef ESTIMATION_MANAGER_SUPPORT_H
+       3             : #define ESTIMATION_MANAGER_SUPPORT_H
+       4             : 
+       5             : /* includes //{ */
+       6             : 
+       7             : #include <string.h>
+       8             : 
+       9             : #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+      10             : 
+      11             : #include <geometry_msgs/TransformStamped.h>
+      12             : #include <geometry_msgs/Pose.h>
+      13             : #include <geometry_msgs/PointStamped.h>
+      14             : 
+      15             : #include <nav_msgs/Odometry.h>
+      16             : 
+      17             : #include <mrs_msgs/UavState.h>
+      18             : 
+      19             : #include <mrs_lib/attitude_converter.h>
+      20             : #include <mrs_lib/transformer.h>
+      21             : 
+      22             : //}
+      23             : 
+      24             : namespace mrs_uav_managers
+      25             : {
+      26             : 
+      27             : namespace estimation_manager
+      28             : {
+      29             : 
+      30             : /*//{ class Support */
+      31             : class Support {
+      32             : 
+      33             : public:
+      34             : 
+      35             :   const static inline std::string waiting_for_string = "\033[0;36mWAITING FOR:\033[0m";
+      36             : 
+      37             :   /*//{ toSnakeCase() */
+      38        2451 :   static std::string toSnakeCase(const std::string& str_in) {
+      39             : 
+      40        2451 :     std::string str(1, tolower(str_in[0]));
+      41             : 
+      42       38539 :     for (auto it = str_in.begin() + 1; it != str_in.end(); ++it) {
+      43       36088 :       if (isupper(*it) && *(it - 1) != '_' && islower(*(it - 1))) {
+      44        2027 :         str += "_";
+      45             :       }
+      46       36088 :       str += *it;
+      47             :     }
+      48             : 
+      49        2451 :     std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+      50             : 
+      51        2451 :     return str;
+      52             :   }
+      53             :   /*//}*/
+      54             : 
+      55             : /* toLowercase() //{ */
+      56             : 
+      57         428 : static std::string toLowercase(const std::string str_in) {
+      58         428 :   std::string str_out = str_in;
+      59         428 :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower);
+      60         428 :   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      314052 :   static tf2::Vector3 rotateVecByHdg(const geometry_msgs::Vector3& vec_in, const double hdg_in) {
+      99             : 
+     100      314052 :     const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+     101             : 
+     102      309235 :     const tf2::Vector3 vec_tf2(vec_in.x, vec_in.y, vec_in.z);
+     103             : 
+     104      308787 :     const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+     105             : 
+     106      617446 :     return vec_rotated;
+     107             :   }
+     108             :   //}
+     109             : 
+     110             :   /* noNans() //{ */
+     111     1224119 :   static bool noNans(const geometry_msgs::TransformStamped& tf) {
+     112             : 
+     113     3668610 :     return (std::isfinite(tf.transform.rotation.x) && std::isfinite(tf.transform.rotation.y) && std::isfinite(tf.transform.rotation.z) &&
+     114     4889252 :             std::isfinite(tf.transform.rotation.w) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.y) &&
+     115     2445223 :             std::isfinite(tf.transform.translation.z));
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* noNans() //{ */
+     120      394056 :   static bool noNans(const geometry_msgs::Quaternion& q) {
+     121             : 
+     122      394056 :     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     1111688 :   static tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) {
+     136             : 
+     137     1111688 :     tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z);
+     138             : 
+     139     1111321 :     tf2::Quaternion q;
+     140     1111390 :     tf2::fromMsg(pose_in.orientation, q);
+     141             : 
+     142     1111169 :     tf2::Transform tf_out(q, position);
+     143             : 
+     144     2218162 :     return tf_out;
+     145             :   }
+     146             : 
+     147             :   //}
+     148             : 
+     149             :   /* poseFromTf2() //{ */
+     150             : 
+     151      752071 :   static geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) {
+     152             : 
+     153      752071 :     geometry_msgs::Pose pose_out;
+     154      749979 :     pose_out.position.x = tf_in.getOrigin().getX();
+     155      749763 :     pose_out.position.y = tf_in.getOrigin().getY();
+     156      750930 :     pose_out.position.z = tf_in.getOrigin().getZ();
+     157             : 
+     158      751955 :     pose_out.orientation = tf2::toMsg(tf_in.getRotation());
+     159             : 
+     160      752218 :     return pose_out;
+     161             :   }
+     162             : 
+     163             :   //}
+     164             : 
+     165             :   /* msgFromTf2() //{ */
+     166      358940 :   static geometry_msgs::Transform msgFromTf2(const tf2::Transform& tf_in) {
+     167             : 
+     168      358940 :     geometry_msgs::Transform tf_out;
+     169      358940 :     tf_out.translation.x = tf_in.getOrigin().getX();
+     170      358940 :     tf_out.translation.y = tf_in.getOrigin().getY();
+     171      358940 :     tf_out.translation.z = tf_in.getOrigin().getZ();
+     172      358940 :     tf_out.rotation = tf2::toMsg(tf_in.getRotation());
+     173             : 
+     174      358940 :     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      741558 :   static geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) {
+     194             : 
+     195      741558 :     geometry_msgs::Vector3 vec_out;
+     196      741348 :     vec_out.x = point_in.x;
+     197      741348 :     vec_out.y = point_in.y;
+     198      741348 :     vec_out.z = point_in.z;
+     199             : 
+     200      741348 :     return vec_out;
+     201             :   }
+     202             : 
+     203             :   //}
+     204             : 
+     205             :   /*//{ rotateVector() */
+     206      369216 :   static geometry_msgs::Vector3 rotateVector(const geometry_msgs::Vector3& vec_in, const geometry_msgs::Quaternion& q_in) {
+     207             : 
+     208             :     try {
+     209      369216 :       Eigen::Matrix3d        R = mrs_lib::AttitudeConverter(q_in);
+     210      369189 :       Eigen::Vector3d        vec_in_eigen(vec_in.x, vec_in.y, vec_in.z);
+     211      369105 :       Eigen::Vector3d        vec_eigen_rotated = R * vec_in_eigen;
+     212      369121 :       geometry_msgs::Vector3 vec_out;
+     213      368769 :       vec_out.x = vec_eigen_rotated[0];
+     214      368864 :       vec_out.y = vec_eigen_rotated[1];
+     215      368967 :       vec_out.z = vec_eigen_rotated[2];
+     216      369010 :       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      368195 :   static nav_msgs::Odometry uavStateToOdom(const mrs_msgs::UavState& uav_state) {
+     227      368195 :     nav_msgs::Odometry odom;
+     228      368187 :     odom.header              = uav_state.header;
+     229      368181 :     odom.child_frame_id      = uav_state.child_frame_id;
+     230      368177 :     odom.pose.pose           = uav_state.pose;
+     231      368177 :     odom.twist.twist.angular = uav_state.velocity.angular;
+     232             : 
+     233      368177 :     tf2::Quaternion q;
+     234      368178 :     tf2::fromMsg(odom.pose.pose.orientation, q);
+     235      368175 :     odom.twist.twist.linear = Support::rotateVector(uav_state.velocity.linear, mrs_lib::AttitudeConverter(q.inverse()));
+     236             : 
+     237      736104 :     return odom;
+     238             :   }
+     239             :   /*//}*/
+     240             : 
+     241             :   /*//{ getPoseDiff() */
+     242           5 :   static geometry_msgs::Pose getPoseDiff(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) {
+     243             : 
+     244           5 :     tf2::Vector3 v1, v2;
+     245           5 :     tf2::fromMsg(p1.position, v1);
+     246           5 :     tf2::fromMsg(p2.position, v2);
+     247           5 :     const tf2::Vector3 v3 = v1 - v2;
+     248             : 
+     249           5 :     tf2::Quaternion q1, q2;
+     250           5 :     tf2::fromMsg(p1.orientation, q1);
+     251           5 :     tf2::fromMsg(p2.orientation, q2);
+     252           5 :     tf2::Quaternion q3 = q2 * q1.inverse();
+     253           5 :     q3.normalize();
+     254             : 
+     255           5 :     geometry_msgs::Pose pose_diff;
+     256           5 :     tf2::toMsg(v3, pose_diff.position);
+     257           5 :     pose_diff.orientation = tf2::toMsg(q3);
+     258             : 
+     259          10 :     return pose_diff;
+     260             :   }
+     261             :   /*//}*/
+     262             : 
+     263             :   /*//{ applyPoseDiff() */
+     264      169146 :   static geometry_msgs::Pose applyPoseDiff(const geometry_msgs::Pose& pose_in, const geometry_msgs::Pose& pose_diff) {
+     265             : 
+     266      169146 :     tf2::Vector3    pos_in;
+     267      169146 :     tf2::Quaternion q_in;
+     268      169146 :     tf2::fromMsg(pose_in.position, pos_in);
+     269      169146 :     tf2::fromMsg(pose_in.orientation, q_in);
+     270             : 
+     271      169146 :     tf2::Vector3    pos_diff;
+     272      169146 :     tf2::Quaternion q_diff;
+     273      169146 :     tf2::fromMsg(pose_diff.position, pos_diff);
+     274      169146 :     tf2::fromMsg(pose_diff.orientation, q_diff);
+     275             : 
+     276      169146 :     const tf2::Vector3    pos_out = tf2::quatRotate(q_diff.inverse(), (pos_in - pos_diff));
+     277      169146 :     const tf2::Quaternion q_out   = q_diff.inverse() * q_in;
+     278             : 
+     279      169146 :     geometry_msgs::Pose pose_out;
+     280      169146 :     tf2::toMsg(pos_out, pose_out.position);
+     281      169146 :     pose_out.orientation = tf2::toMsg(q_out);
+     282             : 
+     283      338292 :     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         333 :   static bool isStringInVector(const std::string& value, const std::vector<std::string>& str_vec) {
+     300         333 :     return std::find(str_vec.begin(), str_vec.end(), value) != str_vec.end();
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /*//{ frameIdToEstimatorName() */
+     305          10 :   static std::string frameIdToEstimatorName(const std::string& str_in) {
+     306          20 :     const std::string str_tmp = str_in.substr(str_in.find("/")+1, str_in.size());
+     307          20 :     return str_tmp.substr(0, str_tmp.find("_origin") );
+     308             :   }
+     309             :   /*//}*/
+     310             : 
+     311             : private:
+     312             :   Support() {
+     313             :   }
+     314             : };
+     315             : /*//}*/
+     316             : 
+     317             : }  // namespace estimation_manager
+     318             : 
+     319             : }  // namespace mrs_uav_managers
+     320             : 
+     321             : #endif  // ESTIMATION_MANAGER_SUPPORT_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html new file mode 100644 index 0000000000..4e128092e1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html new file mode 100644 index 0000000000..afe664a9ba --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-06-08 22:13:12Functions: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..9b83ac27dd --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-06-08 22:13:12Functions:61060.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index.html b/mrs_uav_managers/include/mrs_uav_managers/index.html new file mode 100644 index 0000000000..8a14d8df7c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-06-08 22:13:12Functions: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..53ba58a99d --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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&)113
mrs_uav_managers::StateEstimator::~StateEstimator().2113
+
+
+ + + +
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..0a8716b2e6 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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&)113
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::~StateEstimator().2113
+
+
+ + + +
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..292be2277c --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html @@ -0,0 +1,169 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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         113 :   StateEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      62         113 :       : Estimator(state::type, name, frame_id), package_name_(package_name) {
+      63         113 :   }
+      64             : 
+      65         113 :   virtual ~StateEstimator(void) {
+      66         113 :   }
+      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..afe73e66a5 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-06-08 22:13:12Functions: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().2643
+
+
+ + + +
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..bb1e3ec75a --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-06-08 22:13:12Functions: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().2643
+
+
+ + + +
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..f5e7fc1c17 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html @@ -0,0 +1,296 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-06-08 22:13:12Functions: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         643 :   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:19239848.2 %
Date:2024-06-08 22:13:12Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.0%80.0%
+ 		80.0 %192 / 24092.9 %13 / 14
<unnamed>80.0 %192 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..42926cb078 --- /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:19239848.2 %
Date:2024-06-08 22:13:12Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.0%80.0%
+ 		80.0 %192 / 24092.9 %13 / 14
<unnamed>80.0 %192 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail.html b/mrs_uav_managers/include/transform_manager/index-detail.html new file mode 100644 index 0000000000..7bb94459e6 --- /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:19239848.2 %
Date:2024-06-08 22:13:12Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.0%80.0%
+ 		80.0 %192 / 24092.9 %13 / 14
<unnamed>80.0 %192 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-f.html b/mrs_uav_managers/include/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..4ced4744af --- /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:19239848.2 %
Date:2024-06-08 22:13:12Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.0%80.0%
+ 		80.0 %192 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-l.html b/mrs_uav_managers/include/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..8a1e152c89 --- /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:19239848.2 %
Date:2024-06-08 22:13:12Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.0%80.0%
+ 		80.0 %192 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index.html b/mrs_uav_managers/include/transform_manager/index.html new file mode 100644 index 0000000000..d44c6f65c5 --- /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:19239848.2 %
Date:2024-06-08 22:13:12Functions:131968.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
tf_mapping_origin.h +
0.0%
+ 		0.0 %0 / 1580.0 %0 / 5
tf_source.h +
80.0%80.0%
+ 		80.0 %192 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html new file mode 100644 index 0000000000..0336fef143 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-06-08 22:13:12Functions: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..e94e5bf26a --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-06-08 22:13:12Functions: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..fae2eee07a --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html @@ -0,0 +1,474 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-06-08 22:13:12Functions: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:19224080.0 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::TfSource::setIsUtmSource(bool)10
mrs_uav_managers::TfSource::getIsUtmSource()18
mrs_uav_managers::TfSource::getIsUtmBased()20
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)113
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)113
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)113
mrs_uav_managers::TfSource::getName[abi:cxx11]()923
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> > >&)152514
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)197804
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)197850
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)205305
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)205306
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()955671
+
+
+ + + +
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..6a1e760b2c --- /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:19224080.0 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()955671
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)113
mrs_uav_managers::TfSource::getIsUtmBased()20
mrs_uav_managers::TfSource::getIsUtmSource()18
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::TfSource::setIsUtmSource(bool)10
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)113
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)205305
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> > >&)152514
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)197804
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)205306
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)197850
mrs_uav_managers::TfSource::getName[abi:cxx11]()923
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)113
+
+
+ + + +
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..53dc286a7c --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html @@ -0,0 +1,704 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19224080.0 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..7604638ebcc89b62e79bee3c5a6ea0052bdbad6d GIT binary patch literal 2213 zcmV;W2wL}vP)4->0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpb~b`I;%cFQ0osd;#Rs_ZmzAzr#XK7Ww<)p zu-LW+VBP`&M|QLxG?fM3>6vt|$*6_DxULm#2S(;0Q~qxVsHbRUKIpAwpps&+&8V@C zsx4M*1LQVXm#(!*y+CrwBf6ztqG^CZ3}SW-wypuAQ(#{-lsn(`eQ!_Nwk>`7W`WyX z(N6Fv_|Tz84asB7T%>SgeF6dZ@x9(w&+u`%V?EqpiZW1R!c_}QUF+P;4z4jwHskTj zM}3ugZmbEt%<(P;5ft;~V+d4D0hQ~N7P#|mUciZ|}dJ7wSa{oY6 z+{6Anp4)Sa-~H$IM8M({r=AO&)bpCb6?ZGr2;&@_amP3pMh`L!2m+&1$0bq$NPSxb z8c&gRWUlKf&{Gr^NSMbY3}8C~RSi*Gr47TacY(w%ZvjPh0MIqaqp-$&JC6+;=c@1Crn71&>S5XsK-#(ZB5TQB4A9gnUAQx zU6mz@cQ9LP3 zL)S>%L5h6jP+<6~1fHSTEDWLAM=YbrG27k)dw&!dP~?FDuK^Y*@{CRXQX#>3WjP8U zJ{7qPZ6UcgvZqrAnp!md9%>QWL_3D}{>+Iist zI@Z>e?6qRs)3UtoQMfWa8}%sf*mXQ2f6NliNweM`X%z{7a)vSP=O-N37C(UfUI=1o z4U|e|=WJOf><}q%v@|&69;&X+hb3V+Y5cVy49{aGj3CX`!hk~(Q1HY%ZWeBFIYX>| z(2l~loea@=MkY?@8TP=(G)CO-JX4xy0IW#yo%!$)4%C7kvBQw_OoWI5!T8ig= zxm^tFXKh7anudl#G4>D>IvYOxRx3>id;RgWx5uCh5`+gS|bdg$d;L4V9WHuue1oT zY5K@kG2Md;NS$-q^>CVPXC|af^%QvUX#6F84(U5{RVYqAzf_vKmY(9`fX`fq14z9+*V{$3u4jDAs(iejD%fYLI~DcyyCeY~}H@r?{kz3h=K4Qgs9BiQJRNjH{0q zA$$L1XeQPygk~I@|6FKRx65OAS1!eAC+#@5+^+`;vnm9> zBH>%k#S1_o7mtC3T>OtFrEl4~2Q&1mlG1p&aCK5T2g0N@o#K9FQks3VsVRF+fc)9w zFG+L%j}!Y_A43>b^ZIZPkE?tAdbRH#j8mv=4ea6Qp;x}7n9xNr(d7$)5+rsiR19@= zeGgHu0>t4FNxx;n2xoTBp>k#yC2JA}(Rj$fdo@bLu37FGpe|C}zd@?pHR|~3-D~q~ z_^zLiLF~8eW^Ul{J5Ajzd>?exJxx`1v1-1y0h-Z<#@Df&d&As}3DQ@~&91V$i(;ZH zzkHH(dUF2mKKfCp9M{*q n{GhdGeVbyi7ALI6rX}(Zy0g`)>!brh00000NkvXXu0mjfy~r*u 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..2636520434 --- /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:19523284.1 %
Date:2024-06-08 22:13:12Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)1
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)3
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()107
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)110
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)2350
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&)2887
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)23983
+
+
+ + + +
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..8e35b884c7 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19523284.1 %
Date:2024-06-08 22:13:12Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)110
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&)2887
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)2350
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)3
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)23983
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)1
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()107
+
+
+ + + +
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..ea7bc267e5 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.html @@ -0,0 +1,716 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19523284.1 %
Date:2024-06-08 22:13:12Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::validateOdometry(nav_msgs::Odometry_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, double const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::loadDetailedUavModelParams(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
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&)535
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&)737
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&)905
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&)1168
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&)1356
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&)1662
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&)2786
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2840
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&)3997
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&)4010
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&)10242
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)14116
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&)14116
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)14659
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&)86471
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&)99409
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&)109600
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&)117968
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&)148006
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)156855
+
+
+ + + +
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..3a2cfeaaf1 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::idxInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)1662
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)14659
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)156855
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2840
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&)148006
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&)10242
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> >&, double const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiPositionCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)14116
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> >&, double const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::validateControlOutput(mrs_uav_managers::Controller::ControlOutput const&, mrs_uav_managers::control_manager::ControlOutputModalities_t const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109600
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&)99409
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&)14116
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&)3997
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&)117968
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&)535
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&)737
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&)107
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&)1356
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&)86471
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&)4010
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&)905
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&)2786
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&)1168
+
+
+ + + +
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..bf527c9860 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html @@ -0,0 +1,1312 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_uav_managers/control_manager/common.h>
+       2             : 
+       3             : namespace mrs_uav_managers
+       4             : {
+       5             : 
+       6             : namespace control_manager
+       7             : {
+       8             : 
+       9             : /* idxInVector() //{ */
+      10             : 
+      11        1662 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13        4711 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14        4707 :     if (str == vec[i]) {
+      15        1658 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           4 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26       99409 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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       99409 :   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         737 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240         737 :   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         737 :   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         737 :   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         737 :   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         737 :   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         737 :   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         737 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277       10242 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281       10242 :   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       10242 :   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       10242 :   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       10242 :   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       10242 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310      148006 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   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      148006 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460        2840 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462        2840 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464        2840 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466        2840 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471        2840 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472        2197 :     return 0.0;
+     473             :   }
+     474             : 
+     475         643 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477         434 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479         434 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483         209 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485         209 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495         107 : 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         214 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500         107 :   if (custom_config != "") {
+     501         107 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504         107 :   if (platform_config != "") {
+     505         107 :     param_loader.addYamlFile(platform_config);
+     506             :   }
+     507             : 
+     508             :   double mass;
+     509             :   double arm_length;
+     510             :   double body_height;
+     511             :   double force_constant;
+     512             :   double torque_constant;
+     513             :   double prop_radius;
+     514             :   double rpm_min;
+     515             :   double rpm_max;
+     516             : 
+     517         107 :   bool detailed_loaded = true;
+     518             : 
+     519         107 :   detailed_loaded *= param_loader.loadParam("uav_mass", mass, 0.0);
+     520             : 
+     521         107 :   detailed_loaded *= param_loader.loadParam("model_params/arm_length", arm_length, 0.0);
+     522         107 :   detailed_loaded *= param_loader.loadParam("model_params/body_height", body_height, 0.0);
+     523             : 
+     524         107 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/force_constant", force_constant, 0.0);
+     525         107 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant, 0.0);
+     526         107 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius, 0.0);
+     527         107 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min, 0.0);
+     528         107 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max, 0.0);
+     529             : 
+     530         214 :   Eigen::MatrixXd allocation_matrix;
+     531             : 
+     532         107 :   detailed_loaded *= param_loader.loadMatrixDynamic("model_params/propulsion/allocation_matrix", allocation_matrix, Eigen::Matrix4d::Identity(), 4, -1);
+     533             : 
+     534         107 :   if (!detailed_loaded) {
+     535           0 :     ROS_WARN(
+     536             :         "[%s]: detailed UAV model params not loaded, missing some parameters. This will not permit operations when ACTUATORS or CONTROL_GROUP control "
+     537             :         "outputs would be possible.",
+     538             :         node_name.c_str());
+     539           0 :     return {};
+     540             :   } else {
+     541         107 :     ROS_INFO("[%s]: detailed UAV model params successfully loaded.", node_name.c_str());
+     542             :   }
+     543             : 
+     544         107 :   int n_motors = allocation_matrix.cols();
+     545             : 
+     546         214 :   DetailedModelParams_t model_params;
+     547             : 
+     548         107 :   model_params.arm_length  = arm_length;
+     549         107 :   model_params.body_height = body_height;
+     550         107 :   model_params.prop_radius = prop_radius;
+     551             : 
+     552         107 :   Eigen::Matrix3d inertia_matrix;
+     553             : 
+     554         107 :   bool inertia_loaded = param_loader.loadMatrixStatic("model_params/inertia_matrix", inertia_matrix, Eigen::Matrix3d::Identity());
+     555             : 
+     556         107 :   if (inertia_loaded) {
+     557             : 
+     558           0 :     model_params.inertia = inertia_matrix;
+     559           0 :     ROS_INFO("[%s]: inertia matrix loaded from config file:", node_name.c_str());
+     560           0 :     ROS_INFO_STREAM(model_params.inertia);
+     561             : 
+     562             :   } else {
+     563             : 
+     564         107 :     ROS_INFO("[%s]: inertia matrix missing in the config file, computing it from the other params.", node_name.c_str());
+     565             : 
+     566             :     // create the inertia matrix
+     567         107 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     568         107 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     569         107 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     570         107 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     571             : 
+     572         107 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     573         107 :     ROS_INFO_STREAM(model_params.inertia);
+     574             :   }
+     575             : 
+     576             :   // create the force-torque allocation matrix
+     577         107 :   model_params.force_torque_mixer = allocation_matrix;
+     578         107 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     579         107 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     580         107 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     581         107 :   model_params.force_torque_mixer.row(3) *= force_constant;
+     582             : 
+     583             :   // | ------- create the control group allocation matrix ------- |
+     584             : 
+     585             :   // pseudoinverse of the force-torque matrix (maximum norm)
+     586             :   Eigen::MatrixXd alloc_tmp =
+     587         214 :       model_params.force_torque_mixer.transpose() * (model_params.force_torque_mixer * model_params.force_torque_mixer.transpose()).inverse();
+     588             : 
+     589             :   // | ------------- normalize the allocation matrix ------------ |
+     590             :   // this will make it match the PX4 control group mixing
+     591             : 
+     592             :   // the first two columns (roll, pitch)
+     593         541 :   for (int i = 0; i < n_motors; i++) {
+     594         434 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     595             :   }
+     596             : 
+     597             :   // the 3rd column (yaw)
+     598         541 :   for (int i = 0; i < n_motors; i++) {
+     599         434 :     if (alloc_tmp(i, 2) > 1e-2) {
+     600         217 :       alloc_tmp(i, 2) = 1.0;
+     601         217 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     602         217 :       alloc_tmp(i, 2) = -1.0;
+     603             :     } else {
+     604           0 :       alloc_tmp(i, 2) = 0.0;
+     605             :     }
+     606             :   }
+     607             : 
+     608             :   // the 4th column (throttle)
+     609         541 :   for (int i = 0; i < n_motors; i++) {
+     610         434 :     alloc_tmp(i, 3) = 1.0;
+     611             :   }
+     612             : 
+     613         107 :   model_params.control_group_mixer = alloc_tmp;
+     614             : 
+     615         107 :   return model_params;
+     616             : }
+     617             : 
+     618             : //}
+     619             : 
+     620             : /* getLowestOutput() //{ */
+     621             : 
+     622       14659 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     623             : 
+     624       14659 :   if (outputs.actuators) {
+     625          16 :     return ACTUATORS_CMD;
+     626             :   }
+     627             : 
+     628       14643 :   if (outputs.control_group) {
+     629          16 :     return CONTROL_GROUP;
+     630             :   }
+     631             : 
+     632       14627 :   if (outputs.attitude_rate) {
+     633       14520 :     return ATTITUDE_RATE;
+     634             :   }
+     635             : 
+     636         107 :   if (outputs.attitude) {
+     637          16 :     return ATTITUDE;
+     638             :   }
+     639             : 
+     640          91 :   if (outputs.acceleration_hdg_rate) {
+     641          10 :     return ACCELERATION_HDG_RATE;
+     642             :   }
+     643             : 
+     644          81 :   if (outputs.acceleration_hdg) {
+     645          10 :     return ACCELERATION_HDG;
+     646             :   }
+     647             : 
+     648          71 :   if (outputs.velocity_hdg_rate) {
+     649          41 :     return VELOCITY_HDG_RATE;
+     650             :   }
+     651             : 
+     652          30 :   if (outputs.velocity_hdg) {
+     653          20 :     return VELOCITY_HDG;
+     654             :   }
+     655             : 
+     656          10 :   return POSITION;
+     657             : }
+     658             : 
+     659             : //}
+     660             : 
+     661             : /* getHighestOutput() //{ */
+     662             : 
+     663           0 : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs) {
+     664             : 
+     665           0 :   if (outputs.position) {
+     666           0 :     return POSITION;
+     667             :   }
+     668             : 
+     669           0 :   if (outputs.velocity_hdg) {
+     670           0 :     return VELOCITY_HDG;
+     671             :   }
+     672             : 
+     673           0 :   if (outputs.velocity_hdg_rate) {
+     674           0 :     return VELOCITY_HDG_RATE;
+     675             :   }
+     676             : 
+     677           0 :   if (outputs.acceleration_hdg) {
+     678           0 :     return ACCELERATION_HDG;
+     679             :   }
+     680             : 
+     681           0 :   if (outputs.acceleration_hdg_rate) {
+     682           0 :     return ACCELERATION_HDG_RATE;
+     683             :   }
+     684             : 
+     685           0 :   if (outputs.attitude) {
+     686           0 :     return ATTITUDE;
+     687             :   }
+     688             : 
+     689           0 :   if (outputs.attitude_rate) {
+     690           0 :     return ATTITUDE_RATE;
+     691             :   }
+     692             : 
+     693           0 :   if (outputs.control_group) {
+     694           0 :     return CONTROL_GROUP;
+     695             :   }
+     696             : 
+     697           0 :   return ACTUATORS_CMD;
+     698             : }
+     699             : 
+     700             : //}
+     701             : 
+     702             : // | -------- extraction of throttle out of hw api cmds ------- |
+     703             : 
+     704             : /* extractThrottle() //{ */
+     705             : 
+     706      156855 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     707             : 
+     708      156855 :   if (!control_output.control_output) {
+     709       21502 :     return {};
+     710             :   }
+     711             : 
+     712      270706 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     713             : }
+     714             : 
+     715             : //}
+     716             : 
+     717             : // | -------------- validation of HW api commands ------------- |
+     718             : 
+     719             : /* validateControlOutput() //{ */
+     720             : 
+     721      109600 : bool validateControlOutput(const Controller::ControlOutput& control_output, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     722             :                            const std::string& var_name) {
+     723             : 
+     724      109600 :   if (!control_output.control_output) {
+     725           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: the optional variable '%s' is not set!!!", node_name.c_str(), var_name.c_str());
+     726           0 :     return false;
+     727             :   }
+     728             : 
+     729      109600 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     730      219200 :   std::variant<std::string>               node_name_var{node_name};
+     731      109600 :   std::variant<std::string>               var_name_var{var_name};
+     732             : 
+     733      109600 :   return std::visit(HwApiValidateVisitor(), control_output.control_output.value(), output_modalities_var, node_name_var, var_name_var);
+     734             : }
+     735             : 
+     736             : //}
+     737             : 
+     738             : /* validateHwApiActuatorCmd() //{ */
+     739             : 
+     740        3997 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     741             : 
+     742       19985 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     743       15988 :     if (!std::isfinite(msg.motors[i])) {
+     744           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.motors[%d]'!!!", node_name.c_str(), var_name.c_str(), int(i));
+     745           0 :       return false;
+     746             :     }
+     747             :   }
+     748             : 
+     749        3997 :   return true;
+     750             : }
+     751             : 
+     752             : //}
+     753             : 
+     754             : /* validateHwApiControlGroupCmd() //{ */
+     755             : 
+     756        4010 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     757             : 
+     758        4010 :   if (!std::isfinite(msg.roll)) {
+     759           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.roll'!!!", node_name.c_str(), var_name.c_str());
+     760           0 :     return false;
+     761             :   }
+     762             : 
+     763        4010 :   if (!std::isfinite(msg.pitch)) {
+     764           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pitch'!!!", node_name.c_str(), var_name.c_str());
+     765           0 :     return false;
+     766             :   }
+     767             : 
+     768        4010 :   if (!std::isfinite(msg.yaw)) {
+     769           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.yaw'!!!", node_name.c_str(), var_name.c_str());
+     770           0 :     return false;
+     771             :   }
+     772             : 
+     773        4010 :   if (!std::isfinite(msg.throttle)) {
+     774           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     775           0 :     return false;
+     776             :   }
+     777             : 
+     778        4010 :   return true;
+     779             : }
+     780             : 
+     781             : //}
+     782             : 
+     783             : /* validateHwApiAttitudeCmd() //{ */
+     784             : 
+     785      117968 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     786             : 
+     787             :   // check the orientation
+     788             : 
+     789      117968 :   if (!std::isfinite(msg.orientation.x)) {
+     790           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     791           0 :     return false;
+     792             :   }
+     793             : 
+     794      117968 :   if (!std::isfinite(msg.orientation.y)) {
+     795           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     796           0 :     return false;
+     797             :   }
+     798             : 
+     799      117968 :   if (!std::isfinite(msg.orientation.z)) {
+     800           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     801           0 :     return false;
+     802             :   }
+     803             : 
+     804      117968 :   if (!std::isfinite(msg.orientation.w)) {
+     805           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     806           0 :     return false;
+     807             :   }
+     808             : 
+     809             :   // check the throttle
+     810             : 
+     811      117968 :   if (!std::isfinite(msg.throttle)) {
+     812           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     813           0 :     return false;
+     814             :   }
+     815             : 
+     816      117968 :   return true;
+     817             : }
+     818             : 
+     819             : //}
+     820             : 
+     821             : /* validateHwApiAttitudeRateCmd() //{ */
+     822             : 
+     823       86471 : bool validateHwApiAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     824             : 
+     825             :   // check the body rate
+     826             : 
+     827       86471 :   if (!std::isfinite(msg.body_rate.x)) {
+     828           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.x'!!!", node_name.c_str(), var_name.c_str());
+     829           0 :     return false;
+     830             :   }
+     831             : 
+     832       86471 :   if (!std::isfinite(msg.body_rate.y)) {
+     833           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.y'!!!", node_name.c_str(), var_name.c_str());
+     834           0 :     return false;
+     835             :   }
+     836             : 
+     837       86471 :   if (!std::isfinite(msg.body_rate.z)) {
+     838           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.z'!!!", node_name.c_str(), var_name.c_str());
+     839           0 :     return false;
+     840             :   }
+     841             : 
+     842             :   // check the throttle
+     843             : 
+     844       86471 :   if (!std::isfinite(msg.throttle)) {
+     845           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     846           0 :     return false;
+     847             :   }
+     848             : 
+     849       86471 :   return true;
+     850             : }
+     851             : 
+     852             : //}
+     853             : 
+     854             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     855             : 
+     856        1168 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     857             : 
+     858             :   // | ----------------- check the acceleration ----------------- |
+     859             : 
+     860        1168 :   if (!std::isfinite(msg.acceleration.x)) {
+     861           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     862           0 :     return false;
+     863             :   }
+     864             : 
+     865        1168 :   if (!std::isfinite(msg.acceleration.y)) {
+     866           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     867           0 :     return false;
+     868             :   }
+     869             : 
+     870        1168 :   if (!std::isfinite(msg.acceleration.z)) {
+     871           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     872           0 :     return false;
+     873             :   }
+     874             : 
+     875             :   // check the heading rate
+     876             : 
+     877        1168 :   if (!std::isfinite(msg.heading_rate)) {
+     878           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     879           0 :     return false;
+     880             :   }
+     881             : 
+     882        1168 :   return true;
+     883             : }
+     884             : 
+     885             : //}
+     886             : 
+     887             : /* validateHwApiAccelerationHdgCmd() //{ */
+     888             : 
+     889         905 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     890             : 
+     891             :   // | ----------------- check the acceleration ----------------- |
+     892             : 
+     893         905 :   if (!std::isfinite(msg.acceleration.x)) {
+     894           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     895           0 :     return false;
+     896             :   }
+     897             : 
+     898         905 :   if (!std::isfinite(msg.acceleration.y)) {
+     899           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     900           0 :     return false;
+     901             :   }
+     902             : 
+     903         905 :   if (!std::isfinite(msg.acceleration.z)) {
+     904           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     905           0 :     return false;
+     906             :   }
+     907             : 
+     908             :   // check the heading
+     909             : 
+     910         905 :   if (!std::isfinite(msg.heading)) {
+     911           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     912           0 :     return false;
+     913             :   }
+     914             : 
+     915         905 :   return true;
+     916             : }
+     917             : 
+     918             : //}
+     919             : 
+     920             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     921             : 
+     922        2786 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     923             : 
+     924             :   // | ----------------- check the velocity ----------------- |
+     925             : 
+     926        2786 :   if (!std::isfinite(msg.velocity.x)) {
+     927           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     928           0 :     return false;
+     929             :   }
+     930             : 
+     931        2786 :   if (!std::isfinite(msg.velocity.y)) {
+     932           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     933           0 :     return false;
+     934             :   }
+     935             : 
+     936        2786 :   if (!std::isfinite(msg.velocity.z)) {
+     937           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     938           0 :     return false;
+     939             :   }
+     940             : 
+     941             :   // check the heading rate
+     942             : 
+     943        2786 :   if (!std::isfinite(msg.heading_rate)) {
+     944           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     945           0 :     return false;
+     946             :   }
+     947             : 
+     948        2786 :   return true;
+     949             : }
+     950             : 
+     951             : //}
+     952             : 
+     953             : /* validateHwApiVelocityHdgCmd() //{ */
+     954             : 
+     955        1356 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     956             : 
+     957             :   // | ----------------- check the velocity ----------------- |
+     958             : 
+     959        1356 :   if (!std::isfinite(msg.velocity.x)) {
+     960           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     961           0 :     return false;
+     962             :   }
+     963             : 
+     964        1356 :   if (!std::isfinite(msg.velocity.y)) {
+     965           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     966           0 :     return false;
+     967             :   }
+     968             : 
+     969        1356 :   if (!std::isfinite(msg.velocity.z)) {
+     970           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     971           0 :     return false;
+     972             :   }
+     973             : 
+     974             :   // check the heading
+     975             : 
+     976        1356 :   if (!std::isfinite(msg.heading)) {
+     977           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     978           0 :     return false;
+     979             :   }
+     980             : 
+     981        1356 :   return true;
+     982             : }
+     983             : 
+     984             : //}
+     985             : 
+     986             : /* validateHwApiPositionHdgCmd() //{ */
+     987             : 
+     988         535 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     989             : 
+     990             :   // | ----------------- check the position ----------------- |
+     991             : 
+     992         535 :   if (!std::isfinite(msg.position.x)) {
+     993           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.x'!!!", node_name.c_str(), var_name.c_str());
+     994           0 :     return false;
+     995             :   }
+     996             : 
+     997         535 :   if (!std::isfinite(msg.position.y)) {
+     998           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.y'!!!", node_name.c_str(), var_name.c_str());
+     999           0 :     return false;
+    1000             :   }
+    1001             : 
+    1002         535 :   if (!std::isfinite(msg.position.z)) {
+    1003           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.z'!!!", node_name.c_str(), var_name.c_str());
+    1004           0 :     return false;
+    1005             :   }
+    1006             : 
+    1007             :   // check the heading
+    1008             : 
+    1009         535 :   if (!std::isfinite(msg.heading)) {
+    1010           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+    1011           0 :     return false;
+    1012             :   }
+    1013             : 
+    1014         535 :   return true;
+    1015             : }
+    1016             : 
+    1017             : //}
+    1018             : 
+    1019             : // | ------------ initialization of HW api commands ----------- |
+    1020             : 
+    1021             : /* initializeDefaultOutput() //{ */
+    1022             : 
+    1023       14116 : Controller::HwApiOutputVariant initializeDefaultOutput(const ControlOutputModalities_t& possible_outputs, const mrs_msgs::UavState& uav_state,
+    1024             :                                                        const double& min_throttle, const double& n_motors) {
+    1025             : 
+    1026       14116 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1027             : 
+    1028       14116 :   Controller::HwApiOutputVariant output;
+    1029             : 
+    1030       14116 :   switch (lowest_output) {
+    1031           0 :     case ACTUATORS_CMD: {
+    1032           0 :       output = mrs_msgs::HwApiActuatorCmd();
+    1033           0 :       break;
+    1034             :     }
+    1035           0 :     case CONTROL_GROUP: {
+    1036           0 :       output = mrs_msgs::HwApiControlGroupCmd();
+    1037           0 :       break;
+    1038             :     }
+    1039       14116 :     case ATTITUDE_RATE: {
+    1040       14116 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1041       14116 :       break;
+    1042             :     }
+    1043           0 :     case ATTITUDE: {
+    1044           0 :       output = mrs_msgs::HwApiAttitudeCmd();
+    1045           0 :       break;
+    1046             :     }
+    1047           0 :     case ACCELERATION_HDG_RATE: {
+    1048           0 :       output = mrs_msgs::HwApiAccelerationHdgRateCmd();
+    1049           0 :       break;
+    1050             :     }
+    1051           0 :     case ACCELERATION_HDG: {
+    1052           0 :       output = mrs_msgs::HwApiAccelerationHdgCmd();
+    1053           0 :       break;
+    1054             :     }
+    1055           0 :     case VELOCITY_HDG_RATE: {
+    1056           0 :       output = mrs_msgs::HwApiVelocityHdgRateCmd();
+    1057           0 :       break;
+    1058             :     }
+    1059           0 :     case VELOCITY_HDG: {
+    1060           0 :       output = mrs_msgs::HwApiVelocityHdgCmd();
+    1061           0 :       break;
+    1062             :     }
+    1063           0 :     case POSITION: {
+    1064           0 :       output = mrs_msgs::HwApiPositionCmd();
+    1065           0 :       break;
+    1066             :     }
+    1067             :   }
+    1068             : 
+    1069       28232 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1070       14116 :   std::variant<double>             min_throttle_var{min_throttle};
+    1071       14116 :   std::variant<double>             n_motors_var{n_motors};
+    1072             : 
+    1073       14116 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1074             : 
+    1075       28232 :   return output;
+    1076             : }  // namespace mrs_uav_managers
+    1077             : 
+    1078             : //}
+    1079             : 
+    1080             : /* initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg) //{ */
+    1081             : 
+    1082           0 : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors) {
+    1083             : 
+    1084           0 :   msg.stamp = ros::Time::now();
+    1085             : 
+    1086           0 :   for (int i = 0; i < n_motors; i++) {
+    1087           0 :     msg.motors.push_back(min_throttle);
+    1088             :   }
+    1089           0 : }
+    1090             : 
+    1091             : //}
+    1092             : 
+    1093             : /* initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg) //{ */
+    1094             : 
+    1095           0 : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle) {
+    1096             : 
+    1097           0 :   msg.stamp = ros::Time::now();
+    1098             : 
+    1099           0 :   msg.roll     = 0;
+    1100           0 :   msg.pitch    = 0;
+    1101           0 :   msg.yaw      = 0;
+    1102           0 :   msg.throttle = min_throttle;
+    1103           0 : }
+    1104             : 
+    1105             : //}
+    1106             : 
+    1107             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg) //{ */
+    1108             : 
+    1109       14116 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1110             : 
+    1111       14116 :   msg.stamp = ros::Time::now();
+    1112             : 
+    1113       14116 :   msg.body_rate.x = 0;
+    1114       14116 :   msg.body_rate.y = 0;
+    1115       14116 :   msg.body_rate.z = 0;
+    1116       14116 :   msg.throttle    = min_throttle;
+    1117       14116 : }
+    1118             : 
+    1119             : //}
+    1120             : 
+    1121             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg) //{ */
+    1122             : 
+    1123           0 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle) {
+    1124             : 
+    1125           0 :   msg.stamp = ros::Time::now();
+    1126             : 
+    1127           0 :   msg.orientation = uav_state.pose.orientation;
+    1128           0 :   msg.throttle    = min_throttle;
+    1129           0 : }
+    1130             : 
+    1131             : //}
+    1132             : 
+    1133             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg) //{ */
+    1134             : 
+    1135           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1136             : 
+    1137           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1138           0 :   msg.header.stamp    = ros::Time::now();
+    1139             : 
+    1140           0 :   msg.acceleration.x = 0;
+    1141           0 :   msg.acceleration.y = 0;
+    1142           0 :   msg.acceleration.z = 0;
+    1143           0 :   msg.heading_rate   = 0;
+    1144           0 : }
+    1145             : 
+    1146             : //}
+    1147             : 
+    1148             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg) //{ */
+    1149             : 
+    1150           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1151             : 
+    1152           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1153           0 :   msg.header.stamp    = ros::Time::now();
+    1154             : 
+    1155           0 :   msg.acceleration.x = 0;
+    1156           0 :   msg.acceleration.y = 0;
+    1157           0 :   msg.acceleration.z = 0;
+    1158             : 
+    1159             :   try {
+    1160           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1161             :   }
+    1162           0 :   catch (std::runtime_error& exrun) {
+    1163           0 :     msg.heading = 0;
+    1164             :   }
+    1165           0 : }
+    1166             : 
+    1167             : //}
+    1168             : 
+    1169             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg) //{ */
+    1170             : 
+    1171           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1172             : 
+    1173           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1174           0 :   msg.header.stamp    = ros::Time::now();
+    1175             : 
+    1176           0 :   msg.velocity.x   = 0;
+    1177           0 :   msg.velocity.y   = 0;
+    1178           0 :   msg.velocity.z   = 0;
+    1179           0 :   msg.heading_rate = 0;
+    1180           0 : }
+    1181             : 
+    1182             : //}
+    1183             : 
+    1184             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg) //{ */
+    1185             : 
+    1186           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1187             : 
+    1188           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1189           0 :   msg.header.stamp    = ros::Time::now();
+    1190             : 
+    1191           0 :   msg.velocity.x = 0;
+    1192           0 :   msg.velocity.y = 0;
+    1193           0 :   msg.velocity.z = 0;
+    1194             : 
+    1195             :   try {
+    1196           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1197             :   }
+    1198           0 :   catch (std::runtime_error& exrun) {
+    1199           0 :     msg.heading = 0;
+    1200             :   }
+    1201           0 : }
+    1202             : 
+    1203             : //}
+    1204             : 
+    1205             : /* initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg) //{ */
+    1206             : 
+    1207           0 : void initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1208             : 
+    1209           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1210           0 :   msg.header.stamp    = ros::Time::now();
+    1211             : 
+    1212           0 :   msg.position.x = uav_state.pose.position.x;
+    1213           0 :   msg.position.y = uav_state.pose.position.y;
+    1214           0 :   msg.position.z = uav_state.pose.position.z;
+    1215             : 
+    1216             :   try {
+    1217           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1218             :   }
+    1219           0 :   catch (std::runtime_error& exrun) {
+    1220           0 :     msg.heading = 0;
+    1221             :   }
+    1222           0 : }
+    1223             : 
+    1224             : //}
+    1225             : 
+    1226             : }  // namespace control_manager
+    1227             : 
+    1228             : }  // namespace mrs_uav_managers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html new file mode 100644 index 0000000000..96cf02a42b --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html @@ -0,0 +1,327 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b3860433aecbbd418483ef3e00b664aa390d2497 GIT binary patch literal 2428 zcmV-?34`{DP)5)@ zZU63*!sL0j2U6E4_X=GrB@VBl5c9>hX_@uq3_g+9kCE6Oqx{Ca%=5fV)1*1g2f|Tw zuoFx;$Bk03NOnD@NEBiDp^*|2x~56E%Sdgw5uRy|uRgawmpLxWw7@nmfwqlsj+t#7 z4@MOre82x}TerWT?T@_LE>`oSB;2moH{Kra6on?t)A(>D9{O$c6drT3xO$9|Wc#Ih z7)+~Rtl^dlLbp;6gJ~6vHQZ7$H=v%Ng1S%-gJ~6vGTee|zJq!qt8t=we!G%lFs*`N z!(}jCf-y^lXVs-bU@zOngv4N41!D|1<13l!i6mgC>iM)b)}c7n#aIUoHvp(xFx`%f zojx+Ylc}EY^h#CF`rEM5lHLX zN*C_RU|IzOQg#_%I-#BoV2A2qFbw7kV0ijqFr9)CJ*PPmd(P;(@)H_Go(J-u&Mt6NnNOi!L$m-Gu#5n5?nn~Uw+MC+6BXR0g$!??x#*N z^RNCVBUMfX7Q<3j`I+`AKgSWstmz^pL2CjTdt6uifv(56Gt&1wb}3W$xpt12Y1WIl z>70V29*ax<1l^TbFKHtr`T`vblo6}ylwlMS%;aDf1|mM00bw3TVXp~7BFD!zRSp>= zPSB%^+V!x-I$1vIkc+y6#f{?eBA)!NP%*9cqn8EZb71NXPRPlzHpSY=Z9MiVS@?S{ znn^e@d^cZ=5cF+Zh_GIeN(dQ|@hD_9pP`r*d6iC1qZq;zPe6$C%J7pqXs5&OdxE{_=g$YMz3@7!_lc{j3|5>Wc zGqO)-q=Q39A8XU}>OVqwg5xqPNt1*cQt;VpI$^Gi+6!X?mBrE7Pn7gRyeKK1BTTw{ z^g*n!yMoNjsuD2zUyz=0(>Q-9M_Q}3@zH~=vJ?AOS6xbnowJ@+e8^j$IuD=jTDpMa zws1_{W#O!MWokbe1GB2G4-T*0@2A71p?BVDthpJV3sPb7Vd~ z5DMHt(fS)j2w@sxNf0_CL)GJJP`5sf@jkVt#LxMQUsFIFYW?{-?lsKrHqp-**&_L2 zH5;`yh{4*0+Cdm;_MC!9Ea(K9($;W#TOfHDM$u>?CiZEXW|fVKR$u|zne2)$L|DbS z289AuM-CXB@7+nG^PDw`2~Rfd1=~R$Q@B$2JeSKct}Dec-lZXY>&ftgCy_ToTE@3h zz5l^@D6$vNw#UT4)JeMd!O_j%0vU;2gBuVyVPAKAO1<1ctJ%h6SN+S0J3f|KBDrrn zMz^Fjb9=ayEF)P)JgZ=3Omtmak8xc(dc<^DUq)=wgfb#s*VkjrB0U+gX}GS8h($Uw zqFvXS;s~D5+k{lEmYrO1fwTrpM2JMiCoBB`3e+5m(s+wF-CK%d-6A@Olt9MBLk;x{ z{lpThj*M6;xxS21O?omSU4NrsQdpCojB!oYm9Y<>LDKw=9>4t9P9HLodhVs>)$lSI zu&3TBi4F3_Y*_I<*Wzrpj{B~2clXf6k6!l>IM!SD9GVNglXcGka-Z75%-N|+W>(75 ze05JDogZ$JxaA%kGop8Wy|BukyoKDtd-p=Q>(Ko`mr&yke3m5CADzl_p+a`zT)-8I zY%C7Snf;%n+2_8%6u!N538qv*KHRj}3(;uL&nv>TFZ?noebeXcOIakrS?9bun&b=; z$;v*zKBYlSHIcxqO@WCfa!Fv;zox+SUiI>}L8{G36C7fxSktB3m%uO>2J;0l0bLndAvF582`+W&DgJ~5E2hs$X8wb+sS=vZ8{P5wKhhkrFq5U+$KU><+ zx+i3kUAQZQX%!3zq=nx7&F_Ff3CZmnf%NLlu3#VyEK)g;7PChN(@0bU~5h1)gX5revHoBcmhsCxY zLgG*#>-S7ctiD+eM=cjx1!4CYeflbK(oT4ncXlvJNN_Il^Whg|JPK1m#t~L)Lq|rQ zfr=T8Pe>VS;$bPP$3#uSM#dT)@90?ndGWutjEFAdGOj_wYBJ&{x?7MDwaA!^hi+=h z*s#ykIX@GhUt#R=KlS4piO>ssv}hJM7iVX4yHlIE2&7it}a> zH44uxerpsW;BHny>Y7Nnk~!LGAx`*zT2Jgq{sNL~xd&qxSlBbVGCon*c}`UDt41j5 zMPg{^Sp_29c$W$3@AnguVzs&w#t7f<74N(ANFnn-`R728c}4_S{R)8mkvWV_l+cV# zcy!Kmjn5!82@{i7_hxp+$P#8|jEv1JNl2CPK}{#C1-qxjW1}zF-_FgIPc%&ex=wNY zKlTdjT!3S06Ht*b;e@@L**POa%?ub>XJ(e#YStj6%E&Ps=SU^|kM~@^b}k3WR|u&x uvUA}asf6X7>nRI->Zc?M@b@@I=e~dVah4|DvU>ag0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
<unnamed>41.3 %234 / 56767.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html b/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html new file mode 100644 index 0000000000..7356c545b4 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
<unnamed>41.3 %234 / 56767.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-detail.html b/mrs_uav_managers/src/control_manager/common/index-detail.html new file mode 100644 index 0000000000..1fbf916d27 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
<unnamed>41.3 %234 / 56767.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-sort-f.html b/mrs_uav_managers/src/control_manager/common/index-sort-f.html new file mode 100644 index 0000000000..e735315304 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index-sort-l.html b/mrs_uav_managers/src/control_manager/common/index-sort-l.html new file mode 100644 index 0000000000..cfa6769cbd --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/index.html b/mrs_uav_managers/src/control_manager/common/index.html new file mode 100644 index 0000000000..9180078cb8 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23456741.3 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..95a4fd5032 --- /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:2551367169.5 %
Date:2024-06-08 22:13:12Functions:9511185.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetMinZ(mrs_msgs::Float64StampedSrvRequest_<std::allocator<void> >&, mrs_msgs::Float64StampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)1
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)2
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()4
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)6
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::elandSrv()8
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)12
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)16
mrs_uav_managers::control_manager::ControlManager::isOffboard()18
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)18
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)25
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)27
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)31
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)64
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)93
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)94
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)94
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)94
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)104
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::control_manager::ControlManager::initialize()107
mrs_uav_managers::control_manager::ControlManager::preinitialize()107
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)107
mrs_uav_managers::control_manager::ControlManager::onInit()107
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)108
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)110
mrs_uav_managers::control_manager::ControlManager::ungripSrv()118
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)129
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()151
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)182
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)210
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)211
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()224
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)242
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)245
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()259
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)375
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)424
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)446
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)535
mrs_uav_managers::control_manager::ControlManager::getMass()551
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)643
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)643
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)806
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1714
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2396
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)9533
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)9703
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)10191
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13294
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13297
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)18762
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()18869
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()19134
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)24316
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)66016
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)75687
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)135422
mrs_uav_managers::control_manager::ControlManager::updateTrackers()136027
mrs_uav_managers::control_manager::ControlManager::asyncControl()145544
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)146218
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)146218
mrs_uav_managers::control_manager::ControlManager::publish()146218
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)170192
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)339312
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)438762
+
+
+ + + +
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..fbbfb46e95 --- /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:2551367169.5 %
Date:2024-06-08 22:13:12Functions:9511185.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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)643
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)24316
mrs_uav_managers::control_manager::ControlManager::initialize()107
mrs_uav_managers::control_manager::ControlManager::isOffboard()18
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)446
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2396
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)339312
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)18762
mrs_uav_managers::control_manager::ControlManager::asyncControl()145544
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)75687
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)27
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)94
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)64
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)129
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::preinitialize()107
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)242
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)10191
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)66016
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)107
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()136027
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetMinZ(mrs_msgs::Float64StampedSrvRequest_<std::allocator<void> >&, mrs_msgs::Float64StampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)170192
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()19134
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)245
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)104
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)146218
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)12
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()151
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()18869
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)438762
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)25
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)108
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)16
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)210
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)9703
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1714
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()259
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)1
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)110
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)6
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)182
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)94
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()224
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)211
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)375
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)93
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)9533
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&)806
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)146218
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)424
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)737
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)135422
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)94
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)2
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)643
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)18
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()4
mrs_uav_managers::control_manager::ControlManager::onInit()107
mrs_uav_managers::control_manager::ControlManager::getMass()551
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13294
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13297
mrs_uav_managers::control_manager::ControlManager::publish()146218
mrs_uav_managers::control_manager::ControlManager::elandSrv()8
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::ungripSrv()118
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)535
+
+
+ + + +
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..1ae27a36a4 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,8941 @@ + + + + + + + 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:2551367169.5 %
Date:2024-06-08 22:13:12Functions:9511185.6 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : #include <nodelet/nodelet.h>
+       6             : 
+       7             : #include <mrs_uav_managers/control_manager/common.h>
+       8             : #include <control_manager/output_publisher.h>
+       9             : 
+      10             : #include <mrs_uav_managers/controller.h>
+      11             : #include <mrs_uav_managers/tracker.h>
+      12             : 
+      13             : #include <mrs_msgs/String.h>
+      14             : #include <mrs_msgs/Float64Stamped.h>
+      15             : #include <mrs_msgs/Float64StampedSrv.h>
+      16             : #include <mrs_msgs/ObstacleSectors.h>
+      17             : #include <mrs_msgs/BoolStamped.h>
+      18             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      19             : #include <mrs_msgs/DynamicsConstraints.h>
+      20             : #include <mrs_msgs/ControlError.h>
+      21             : #include <mrs_msgs/GetFloat64.h>
+      22             : #include <mrs_msgs/ValidateReference.h>
+      23             : #include <mrs_msgs/ValidateReferenceList.h>
+      24             : #include <mrs_msgs/TrackerCommand.h>
+      25             : #include <mrs_msgs/EstimatorInput.h>
+      26             : 
+      27             : #include <geometry_msgs/Point32.h>
+      28             : #include <geometry_msgs/TwistStamped.h>
+      29             : #include <geometry_msgs/PoseArray.h>
+      30             : #include <geometry_msgs/Vector3Stamped.h>
+      31             : 
+      32             : #include <nav_msgs/Odometry.h>
+      33             : 
+      34             : #include <sensor_msgs/Joy.h>
+      35             : #include <sensor_msgs/NavSatFix.h>
+      36             : 
+      37             : #include <mrs_lib/safety_zone/safety_zone.h>
+      38             : #include <mrs_lib/profiler.h>
+      39             : #include <mrs_lib/param_loader.h>
+      40             : #include <mrs_lib/utils.h>
+      41             : #include <mrs_lib/mutex.h>
+      42             : #include <mrs_lib/transformer.h>
+      43             : #include <mrs_lib/geometry/misc.h>
+      44             : #include <mrs_lib/geometry/cyclic.h>
+      45             : #include <mrs_lib/attitude_converter.h>
+      46             : #include <mrs_lib/subscribe_handler.h>
+      47             : #include <mrs_lib/msg_extractor.h>
+      48             : #include <mrs_lib/quadratic_throttle_model.h>
+      49             : #include <mrs_lib/publisher_handler.h>
+      50             : #include <mrs_lib/service_client_handler.h>
+      51             : 
+      52             : #include <mrs_msgs/HwApiCapabilities.h>
+      53             : #include <mrs_msgs/HwApiStatus.h>
+      54             : #include <mrs_msgs/HwApiRcChannels.h>
+      55             : 
+      56             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      57             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      58             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      59             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      60             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      61             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      62             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      63             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      64             : #include <mrs_msgs/HwApiPositionCmd.h>
+      65             : 
+      66             : #include <std_msgs/Float64.h>
+      67             : 
+      68             : #include <future>
+      69             : 
+      70             : #include <pluginlib/class_loader.h>
+      71             : 
+      72             : #include <nodelet/loader.h>
+      73             : 
+      74             : #include <eigen3/Eigen/Eigen>
+      75             : 
+      76             : #include <visualization_msgs/Marker.h>
+      77             : #include <visualization_msgs/MarkerArray.h>
+      78             : 
+      79             : #include <mrs_msgs/Reference.h>
+      80             : #include <mrs_msgs/ReferenceStamped.h>
+      81             : #include <mrs_msgs/ReferenceList.h>
+      82             : #include <mrs_msgs/TrajectoryReference.h>
+      83             : 
+      84             : #include <mrs_msgs/ReferenceStampedSrv.h>
+      85             : #include <mrs_msgs/ReferenceStampedSrvRequest.h>
+      86             : #include <mrs_msgs/ReferenceStampedSrvResponse.h>
+      87             : 
+      88             : #include <mrs_msgs/VelocityReferenceStampedSrv.h>
+      89             : #include <mrs_msgs/VelocityReferenceStampedSrvRequest.h>
+      90             : #include <mrs_msgs/VelocityReferenceStampedSrvResponse.h>
+      91             : 
+      92             : #include <mrs_msgs/TransformReferenceSrv.h>
+      93             : #include <mrs_msgs/TransformReferenceSrvRequest.h>
+      94             : #include <mrs_msgs/TransformReferenceSrvResponse.h>
+      95             : 
+      96             : #include <mrs_msgs/TransformPoseSrv.h>
+      97             : #include <mrs_msgs/TransformPoseSrvRequest.h>
+      98             : #include <mrs_msgs/TransformPoseSrvResponse.h>
+      99             : 
+     100             : #include <mrs_msgs/TransformVector3Srv.h>
+     101             : #include <mrs_msgs/TransformVector3SrvRequest.h>
+     102             : #include <mrs_msgs/TransformVector3SrvResponse.h>
+     103             : 
+     104             : #include <mrs_msgs/Float64StampedSrv.h>
+     105             : #include <mrs_msgs/Float64StampedSrvRequest.h>
+     106             : #include <mrs_msgs/Float64StampedSrvResponse.h>
+     107             : 
+     108             : #include <mrs_msgs/Vec4.h>
+     109             : #include <mrs_msgs/Vec4Request.h>
+     110             : #include <mrs_msgs/Vec4Response.h>
+     111             : 
+     112             : #include <mrs_msgs/Vec1.h>
+     113             : #include <mrs_msgs/Vec1Request.h>
+     114             : #include <mrs_msgs/Vec1Response.h>
+     115             : 
+     116             : //}
+     117             : 
+     118             : /* defines //{ */
+     119             : 
+     120             : #define TAU 2 * M_PI
+     121             : #define REF_X 0
+     122             : #define REF_Y 1
+     123             : #define REF_Z 2
+     124             : #define REF_HEADING 3
+     125             : #define ELAND_STR "eland"
+     126             : #define EHOVER_STR "ehover"
+     127             : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+     128             : #define FAILSAFE_STR "failsafe"
+     129             : #define INPUT_UAV_STATE 0
+     130             : #define INPUT_ODOMETRY 1
+     131             : #define RC_DEADBAND 0.2
+     132             : 
+     133             : //}
+     134             : 
+     135             : /* using //{ */
+     136             : 
+     137             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+     138             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+     139             : 
+     140             : using radians  = mrs_lib::geometry::radians;
+     141             : using sradians = mrs_lib::geometry::sradians;
+     142             : 
+     143             : //}
+     144             : 
+     145             : namespace mrs_uav_managers
+     146             : {
+     147             : 
+     148             : namespace control_manager
+     149             : {
+     150             : 
+     151             : /* //{ class ControlManager */
+     152             : 
+     153             : // state machine
+     154             : typedef enum
+     155             : {
+     156             : 
+     157             :   IDLE_STATE,
+     158             :   LANDING_STATE,
+     159             : 
+     160             : } LandingStates_t;
+     161             : 
+     162             : const char* state_names[2] = {"IDLING", "LANDING"};
+     163             : 
+     164             : // state machine
+     165             : typedef enum
+     166             : {
+     167             : 
+     168             :   FCU_FRAME,
+     169             :   RELATIVE_FRAME,
+     170             :   ABSOLUTE_FRAME
+     171             : 
+     172             : } ReferenceFrameType_t;
+     173             : 
+     174             : // state machine
+     175             : typedef enum
+     176             : {
+     177             : 
+     178             :   ESC_NONE_STATE     = 0,
+     179             :   ESC_EHOVER_STATE   = 1,
+     180             :   ESC_ELAND_STATE    = 2,
+     181             :   ESC_FAILSAFE_STATE = 3,
+     182             :   ESC_FINISHED_STATE = 4,
+     183             : 
+     184             : } EscalatingFailsafeStates_t;
+     185             : 
+     186             : /* class ControllerParams() //{ */
+     187             : 
+     188             : class ControllerParams {
+     189             : 
+     190             : public:
+     191             :   ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+     192             :                    bool human_switchable);
+     193             : 
+     194             : public:
+     195             :   double      failsafe_threshold;
+     196             :   double      eland_threshold;
+     197             :   double      odometry_innovation_threshold;
+     198             :   std::string address;
+     199             :   std::string name_space;
+     200             :   bool        human_switchable;
+     201             : };
+     202             : 
+     203         535 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     204         535 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     205             : 
+     206         535 :   this->eland_threshold               = eland_threshold;
+     207         535 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     208         535 :   this->failsafe_threshold            = failsafe_threshold;
+     209         535 :   this->address                       = address;
+     210         535 :   this->name_space                    = name_space;
+     211         535 :   this->human_switchable              = human_switchable;
+     212         535 : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* class TrackerParams() //{ */
+     217             : 
+     218             : class TrackerParams {
+     219             : 
+     220             : public:
+     221             :   TrackerParams(std::string address, std::string name_space, bool human_switchable);
+     222             : 
+     223             : public:
+     224             :   std::string address;
+     225             :   std::string name_space;
+     226             :   bool        human_switchable;
+     227             : };
+     228             : 
+     229         643 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     230             : 
+     231         643 :   this->address          = address;
+     232         643 :   this->name_space       = name_space;
+     233         643 :   this->human_switchable = human_switchable;
+     234         643 : }
+     235             : 
+     236             : //}
+     237             : 
+     238             : class ControlManager : public nodelet::Nodelet {
+     239             : 
+     240             : public:
+     241             :   virtual void onInit();
+     242             : 
+     243             : private:
+     244             :   ros::NodeHandle   nh_;
+     245             :   std::atomic<bool> is_initialized_ = false;
+     246             :   std::string       _uav_name_;
+     247             :   std::string       _body_frame_;
+     248             : 
+     249             :   std::string _custom_config_;
+     250             :   std::string _platform_config_;
+     251             :   std::string _world_config_;
+     252             :   std::string _network_config_;
+     253             : 
+     254             :   // | --------------- dynamic loading of trackers -------------- |
+     255             : 
+     256             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     257             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     258             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     259             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     260             :   std::mutex                                                         mutex_tracker_list_;
+     261             : 
+     262             :   // | ------------- dynamic loading of controllers ------------- |
+     263             : 
+     264             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     265             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     266             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     267             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     268             :   std::mutex                                                            mutex_controller_list_;
+     269             : 
+     270             :   // | ------------------------- HW API ------------------------- |
+     271             : 
+     272             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     273             : 
+     274             :   OutputPublisher control_output_publisher_;
+     275             : 
+     276             :   ControlOutputModalities_t _hw_api_inputs_;
+     277             : 
+     278             :   double desired_uav_state_rate_ = 100.0;
+     279             : 
+     280             :   // this timer will check till we already got the hardware api diagnostics
+     281             :   // then it will trigger the initialization of the controllers and finish
+     282             :   // the initialization of the ControlManager
+     283             :   ros::Timer timer_hw_api_capabilities_;
+     284             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     285             : 
+     286             :   void preinitialize(void);
+     287             :   void initialize(void);
+     288             : 
+     289             :   // | ------------ tracker and controller switching ------------ |
+     290             : 
+     291             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     292             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     293             : 
+     294             :   // the time of last switching of a tracker or a controller
+     295             :   ros::Time  controller_tracker_switch_time_;
+     296             :   std::mutex mutex_controller_tracker_switch_time_;
+     297             : 
+     298             :   // | -------------------- the transformer  -------------------- |
+     299             : 
+     300             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     301             : 
+     302             :   // | ------------------- scope timer logger ------------------- |
+     303             : 
+     304             :   bool                                       scope_timer_enabled_ = false;
+     305             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     306             : 
+     307             :   // | --------------------- general params --------------------- |
+     308             : 
+     309             :   // defines the type of state input: odometry or uav_state mesasge types
+     310             :   int _state_input_;
+     311             : 
+     312             :   // names of important trackers
+     313             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     314             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     315             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     316             : 
+     317             :   // names of important controllers
+     318             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     319             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     320             : 
+     321             :   // joystick control
+     322             :   bool        _joystick_enabled_ = false;
+     323             :   int         _joystick_mode_;
+     324             :   std::string _joystick_tracker_name_;
+     325             :   std::string _joystick_controller_name_;
+     326             :   std::string _joystick_fallback_tracker_name_;
+     327             :   std::string _joystick_fallback_controller_name_;
+     328             : 
+     329             :   // should disarm after emergancy landing?
+     330             :   bool _eland_disarm_enabled_ = false;
+     331             : 
+     332             :   // enabling the emergency handoff -> will disable eland and failsafe
+     333             :   bool _rc_emergency_handoff_ = false;
+     334             : 
+     335             :   // what throttle should be output when null tracker is active?
+     336             :   double _min_throttle_null_tracker_ = 0.0;
+     337             : 
+     338             :   // rates of all the timers
+     339             :   double _status_timer_rate_   = 0;
+     340             :   double _safety_timer_rate_   = 0;
+     341             :   double _elanding_timer_rate_ = 0;
+     342             :   double _failsafe_timer_rate_ = 0;
+     343             :   double _bumper_timer_rate_   = 0;
+     344             : 
+     345             :   bool _snap_trajectory_to_safety_area_ = false;
+     346             : 
+     347             :   // | -------------- uav_state/odometry subscriber ------------- |
+     348             : 
+     349             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     350             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     351             : 
+     352             :   mrs_msgs::UavState uav_state_;
+     353             :   mrs_msgs::UavState previous_uav_state_;
+     354             :   bool               got_uav_state_               = false;
+     355             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     356             :   double             uav_roll_                    = 0;
+     357             :   double             uav_pitch_                   = 0;
+     358             :   double             uav_yaw_                     = 0;
+     359             :   double             uav_heading_                 = 0;
+     360             :   std::mutex         mutex_uav_state_;
+     361             : 
+     362             :   // odometry hiccup detection
+     363             :   double uav_state_avg_dt_        = 1;
+     364             :   double uav_state_hiccup_factor_ = 1;
+     365             :   int    uav_state_count_         = 0;
+     366             : 
+     367             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     368             : 
+     369             :   // | -------------- safety area max z subscriber -------------- |
+     370             : 
+     371             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     372             : 
+     373             :   // | ------------- odometry innovation subscriber ------------- |
+     374             : 
+     375             :   // odometry innovation is published by the odometry node
+     376             :   // it is used to issue eland if the estimator's input is too wonky
+     377             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     378             : 
+     379             :   // | --------------------- common handlers -------------------- |
+     380             : 
+     381             :   // contains handlers that are shared with trackers and controllers
+     382             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     383             : 
+     384             :   // | --------------- tracker and controller IDs --------------- |
+     385             : 
+     386             :   // keeping track of currently active controllers and trackers
+     387             :   unsigned int active_tracker_idx_    = 0;
+     388             :   unsigned int active_controller_idx_ = 0;
+     389             : 
+     390             :   // indeces of some notable trackers
+     391             :   unsigned int _ehover_tracker_idx_               = 0;
+     392             :   unsigned int _landoff_tracker_idx_              = 0;
+     393             :   unsigned int _joystick_tracker_idx_             = 0;
+     394             :   unsigned int _joystick_controller_idx_          = 0;
+     395             :   unsigned int _failsafe_controller_idx_          = 0;
+     396             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     397             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     398             :   unsigned int _null_tracker_idx_                 = 0;
+     399             :   unsigned int _eland_controller_idx_             = 0;
+     400             : 
+     401             :   // | -------------- enabling the output publisher ------------- |
+     402             : 
+     403             :   void              toggleOutput(const bool& input);
+     404             :   std::atomic<bool> output_enabled_ = false;
+     405             : 
+     406             :   // | ----------------------- publishers ----------------------- |
+     407             : 
+     408             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     409             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     410             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     411             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     413             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     414             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     415             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     416             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_nominal_;
+     417             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     418             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     419             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     420             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     421             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     422             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     423             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     424             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     425             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     426             : 
+     427             :   // | --------------------- service servers -------------------- |
+     428             : 
+     429             :   ros::ServiceServer service_server_switch_tracker_;
+     430             :   ros::ServiceServer service_server_switch_controller_;
+     431             :   ros::ServiceServer service_server_reset_tracker_;
+     432             :   ros::ServiceServer service_server_hover_;
+     433             :   ros::ServiceServer service_server_ehover_;
+     434             :   ros::ServiceServer service_server_failsafe_;
+     435             :   ros::ServiceServer service_server_failsafe_escalating_;
+     436             :   ros::ServiceServer service_server_toggle_output_;
+     437             :   ros::ServiceServer service_server_arm_;
+     438             :   ros::ServiceServer service_server_enable_callbacks_;
+     439             :   ros::ServiceServer service_server_set_constraints_;
+     440             :   ros::ServiceServer service_server_use_joystick_;
+     441             :   ros::ServiceServer service_server_use_safety_area_;
+     442             :   ros::ServiceServer service_server_emergency_reference_;
+     443             :   ros::ServiceServer service_server_pirouette_;
+     444             :   ros::ServiceServer service_server_eland_;
+     445             :   ros::ServiceServer service_server_parachute_;
+     446             :   ros::ServiceServer service_server_set_min_z_;
+     447             : 
+     448             :   // human callbable services for references
+     449             :   ros::ServiceServer service_server_goto_;
+     450             :   ros::ServiceServer service_server_goto_fcu_;
+     451             :   ros::ServiceServer service_server_goto_relative_;
+     452             :   ros::ServiceServer service_server_goto_altitude_;
+     453             :   ros::ServiceServer service_server_set_heading_;
+     454             :   ros::ServiceServer service_server_set_heading_relative_;
+     455             : 
+     456             :   // the reference service and subscriber
+     457             :   ros::ServiceServer                                    service_server_reference_;
+     458             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     459             : 
+     460             :   // the velocity reference service and subscriber
+     461             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     462             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     463             : 
+     464             :   // trajectory tracking
+     465             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     466             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     467             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     468             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     469             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     470             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     471             : 
+     472             :   // transform service servers
+     473             :   ros::ServiceServer service_server_transform_reference_;
+     474             :   ros::ServiceServer service_server_transform_pose_;
+     475             :   ros::ServiceServer service_server_transform_vector3_;
+     476             : 
+     477             :   // safety area services
+     478             :   ros::ServiceServer service_server_validate_reference_;
+     479             :   ros::ServiceServer service_server_validate_reference_2d_;
+     480             :   ros::ServiceServer service_server_validate_reference_list_;
+     481             : 
+     482             :   // bumper service servers
+     483             :   ros::ServiceServer service_server_bumper_enabler_;
+     484             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     485             : 
+     486             :   // service clients
+     487             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     488             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     489             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     490             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     491             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     492             : 
+     493             :   // safety area min z servers
+     494             :   ros::ServiceServer service_server_get_min_z_;
+     495             : 
+     496             :   // | --------- trackers' and controllers' last results -------- |
+     497             : 
+     498             :   // the last result of an active tracker
+     499             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     500             :   std::mutex                              mutex_last_tracker_cmd_;
+     501             : 
+     502             :   // the last result of an active controller
+     503             :   Controller::ControlOutput last_control_output_;
+     504             :   std::mutex                mutex_last_control_output_;
+     505             : 
+     506             :   // | -------------- HW API diagnostics subscriber ------------- |
+     507             : 
+     508             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     509             : 
+     510             :   bool offboard_mode_          = false;
+     511             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     512             :   bool armed_                  = false;
+     513             : 
+     514             :   // | -------------------- throttle and mass ------------------- |
+     515             : 
+     516             :   // throttle mass estimation during eland
+     517             :   double    throttle_mass_estimate_   = 0;
+     518             :   bool      throttle_under_threshold_ = false;
+     519             :   ros::Time throttle_mass_estimate_first_time_;
+     520             : 
+     521             :   // | ---------------------- safety params --------------------- |
+     522             : 
+     523             :   // failsafe when tilt error is too large
+     524             :   bool   _tilt_error_disarm_enabled_;
+     525             :   double _tilt_error_disarm_timeout_;
+     526             :   double _tilt_error_disarm_threshold_;
+     527             : 
+     528             :   ros::Time tilt_error_disarm_time_;
+     529             :   bool      tilt_error_disarm_over_thr_ = false;
+     530             : 
+     531             :   // elanding when tilt error is too large
+     532             :   bool   _tilt_limit_eland_enabled_;
+     533             :   double _tilt_limit_eland_ = 0;  // [rad]
+     534             : 
+     535             :   // disarming when tilt error is too large
+     536             :   bool   _tilt_limit_disarm_enabled_;
+     537             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     538             : 
+     539             :   // elanding when yaw error is too large
+     540             :   bool   _yaw_error_eland_enabled_;
+     541             :   double _yaw_error_eland_ = 0;  // [rad]
+     542             : 
+     543             :   // keeping track of control errors
+     544             :   std::optional<double> tilt_error_ = 0;
+     545             :   std::optional<double> yaw_error_  = 0;
+     546             :   std::mutex            mutex_attitude_error_;
+     547             : 
+     548             :   std::optional<Eigen::Vector3d> position_error_;
+     549             :   std::mutex                     mutex_position_error_;
+     550             : 
+     551             :   // control error for triggering failsafe, eland, etc.
+     552             :   // this filled with the current controllers failsafe threshold
+     553             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     554             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     555             :   bool   _odometry_innovation_check_enabled_ = false;
+     556             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     557             : 
+     558             :   bool callbacks_enabled_ = true;
+     559             : 
+     560             :   // | ------------------------ parachute ----------------------- |
+     561             : 
+     562             :   bool _parachute_enabled_ = false;
+     563             : 
+     564             :   std::tuple<bool, std::string> deployParachute(void);
+     565             :   bool                          parachuteSrv(void);
+     566             : 
+     567             :   // | ----------------------- safety area ---------------------- |
+     568             : 
+     569             :   // safety area
+     570             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     571             : 
+     572             :   std::atomic<bool> use_safety_area_ = false;
+     573             : 
+     574             :   std::string _safety_area_horizontal_frame_;
+     575             :   std::string _safety_area_vertical_frame_;
+     576             : 
+     577             :   std::atomic<double> _safety_area_min_z_ = 0;
+     578             : 
+     579             :   double _safety_area_max_z_ = 0;
+     580             : 
+     581             :   // safety area routines
+     582             :   // those are passed to trackers using the common_handlers object
+     583             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     584             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     585             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     586             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     587             :   double getMinZ(const std::string& frame_id);
+     588             :   double getMaxZ(const std::string& frame_id);
+     589             : 
+     590             :   // | ------------------------ callbacks ----------------------- |
+     591             : 
+     592             :   // topic callbacks
+     593             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     594             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     595             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     596             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     597             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     598             : 
+     599             :   // topic timeouts
+     600             :   void timeoutUavState(const double& missing_for);
+     601             : 
+     602             :   // switching controller and tracker services
+     603             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     604             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     605             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     606             : 
+     607             :   // reference callbacks
+     608             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     609             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     610             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     611             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     612             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     613             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     614             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     615             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     616             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     617             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     618             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     619             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     620             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     621             : 
+     622             :   // safety callbacks
+     623             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     624             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     625             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     626             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     627             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     628             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackSetMinZ(mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res);
+     634             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     635             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     636             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     637             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     638             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     639             : 
+     640             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     641             : 
+     642             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     643             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     644             :   bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+     645             : 
+     646             :   // transformation callbacks
+     647             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     648             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     649             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     650             : 
+     651             :   // | ----------------------- constraints ---------------------- |
+     652             : 
+     653             :   // sets constraints to all trackers
+     654             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     655             : 
+     656             :   // constraints management
+     657             :   bool              got_constraints_ = false;
+     658             :   std::mutex        mutex_constraints_;
+     659             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     660             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     661             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     662             :   std::atomic<bool> constraints_being_enforced_ = false;
+     663             : 
+     664             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     665             : 
+     666             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     667             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     668             : 
+     669             :   // | ------------------ emergency triggered? ------------------ |
+     670             : 
+     671             :   std::atomic<bool> failsafe_triggered_ = false;
+     672             :   std::atomic<bool> eland_triggered_    = false;
+     673             : 
+     674             :   // | ------------------------- timers ------------------------- |
+     675             : 
+     676             :   // timer for regular status publishing
+     677             :   ros::Timer timer_status_;
+     678             :   void       timerStatus(const ros::TimerEvent& event);
+     679             : 
+     680             :   // timer for issuing the failsafe landing
+     681             :   ros::Timer timer_failsafe_;
+     682             :   void       timerFailsafe(const ros::TimerEvent& event);
+     683             : 
+     684             :   // oneshot timer for running controllers and trackers
+     685             :   void              asyncControl(void);
+     686             :   std::atomic<bool> running_async_control_ = false;
+     687             :   std::future<void> async_control_result_;
+     688             : 
+     689             :   // timer for issuing emergancy landing
+     690             :   ros::Timer timer_eland_;
+     691             :   void       timerEland(const ros::TimerEvent& event);
+     692             : 
+     693             :   // timer for regular checking of controller errors
+     694             :   ros::Timer        timer_safety_;
+     695             :   void              timerSafety(const ros::TimerEvent& event);
+     696             :   std::atomic<bool> running_safety_timer_        = false;
+     697             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     698             : 
+     699             :   // timer for issuing the pirouette
+     700             :   ros::Timer timer_pirouette_;
+     701             :   void       timerPirouette(const ros::TimerEvent& event);
+     702             : 
+     703             :   // | --------------------- obstacle bumper -------------------- |
+     704             : 
+     705             :   // bumper timer
+     706             :   ros::Timer timer_bumper_;
+     707             :   void       timerBumper(const ros::TimerEvent& event);
+     708             : 
+     709             :   // bumper subscriber
+     710             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     711             : 
+     712             :   bool        _bumper_switch_tracker_    = false;
+     713             :   bool        _bumper_switch_controller_ = false;
+     714             :   std::string _bumper_tracker_name_;
+     715             :   std::string _bumper_controller_name_;
+     716             :   std::string bumper_previous_tracker_;
+     717             :   std::string bumper_previous_controller_;
+     718             : 
+     719             :   std::atomic<bool> bumper_enabled_   = false;
+     720             :   std::atomic<bool> bumper_repulsing_ = false;
+     721             : 
+     722             :   bool _bumper_horizontal_derive_from_dynamics_;
+     723             :   bool _bumper_vertical_derive_from_dynamics_;
+     724             : 
+     725             :   double _bumper_horizontal_distance_ = 0;
+     726             :   double _bumper_vertical_distance_   = 0;
+     727             : 
+     728             :   double _bumper_horizontal_overshoot_ = 0;
+     729             :   double _bumper_vertical_overshoot_   = 0;
+     730             : 
+     731             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     732             :   void bumperPushFromObstacle(void);
+     733             : 
+     734             :   // | --------------- safety checks and failsafes -------------- |
+     735             : 
+     736             :   // escalating failsafe (eland -> failsafe -> disarm)
+     737             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     738             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     739             :   double                     _escalating_failsafe_timeout_         = 0;
+     740             :   ros::Time                  escalating_failsafe_time_;
+     741             :   bool                       _escalating_failsafe_ehover_   = false;
+     742             :   bool                       _escalating_failsafe_eland_    = false;
+     743             :   bool                       _escalating_failsafe_failsafe_ = false;
+     744             :   double                     _rc_escalating_failsafe_threshold_;
+     745             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     746             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     747             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     748             : 
+     749             :   std::string _tracker_error_action_;
+     750             : 
+     751             :   // emergancy landing state machine
+     752             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     753             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     754             :   std::mutex      mutex_landing_state_machine_;
+     755             :   void            changeLandingState(LandingStates_t new_state);
+     756             :   double          _uav_mass_ = 0;
+     757             :   double          _elanding_cutoff_mass_factor_;
+     758             :   double          _elanding_cutoff_timeout_;
+     759             :   double          landing_uav_mass_ = 0;
+     760             : 
+     761             :   // initial body disturbance loaded from params
+     762             :   double _initial_body_disturbance_x_ = 0;
+     763             :   double _initial_body_disturbance_y_ = 0;
+     764             : 
+     765             :   // profiling
+     766             :   mrs_lib::Profiler profiler_;
+     767             :   bool              _profiler_enabled_ = false;
+     768             : 
+     769             :   // diagnostics publishing
+     770             :   void       publishDiagnostics(void);
+     771             :   std::mutex mutex_diagnostics_;
+     772             : 
+     773             :   void                                             ungripSrv(void);
+     774             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     775             : 
+     776             :   bool isFlyingNormally(void);
+     777             : 
+     778             :   // | ------------------------ pirouette ----------------------- |
+     779             : 
+     780             :   bool       _pirouette_enabled_ = false;
+     781             :   double     _pirouette_speed_;
+     782             :   double     _pirouette_timer_rate_;
+     783             :   std::mutex mutex_pirouette_;
+     784             :   double     pirouette_initial_heading_;
+     785             :   double     pirouette_iterator_;
+     786             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     787             : 
+     788             :   // | -------------------- joystick control -------------------- |
+     789             : 
+     790             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     791             : 
+     792             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     793             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     794             : 
+     795             :   // joystick buttons mappings
+     796             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     797             : 
+     798             :   // channel numbers and channel multipliers
+     799             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     800             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     801             : 
+     802             :   ros::Timer timer_joystick_;
+     803             :   void       timerJoystick(const ros::TimerEvent& event);
+     804             :   double     _joystick_timer_rate_ = 0;
+     805             : 
+     806             :   double _joystick_carrot_distance_ = 0;
+     807             : 
+     808             :   ros::Time joystick_start_press_time_;
+     809             :   bool      joystick_start_pressed_ = false;
+     810             : 
+     811             :   ros::Time joystick_back_press_time_;
+     812             :   bool      joystick_back_pressed_ = false;
+     813             :   bool      joystick_goto_enabled_ = false;
+     814             : 
+     815             :   bool      joystick_failsafe_pressed_ = false;
+     816             :   ros::Time joystick_failsafe_press_time_;
+     817             : 
+     818             :   bool      joystick_eland_pressed_ = false;
+     819             :   ros::Time joystick_eland_press_time_;
+     820             : 
+     821             :   // | ------------------- RC joystick control ------------------ |
+     822             : 
+     823             :   // listening to the RC channels as told by pixhawk
+     824             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     825             : 
+     826             :   // the RC channel mapping of the main 4 control signals
+     827             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     828             : 
+     829             :   bool              _rc_goto_enabled_               = false;
+     830             :   std::atomic<bool> rc_goto_active_                 = false;
+     831             :   double            rc_joystick_channel_last_value_ = 0.5;
+     832             :   bool              rc_joystick_channel_was_low_    = false;
+     833             :   int               _rc_joystick_channel_           = 0;
+     834             : 
+     835             :   double _rc_horizontal_speed_ = 0;
+     836             :   double _rc_vertical_speed_   = 0;
+     837             :   double _rc_heading_rate_     = 0;
+     838             : 
+     839             :   // | ------------------- trajectory loading ------------------- |
+     840             : 
+     841             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     842             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     843             : 
+     844             :   // | --------------------- other routines --------------------- |
+     845             : 
+     846             :   // this is called to update the trackers and to receive position control command from the active one
+     847             :   void updateTrackers(void);
+     848             : 
+     849             :   // this is called to update the controllers and to receive attitude control command from the active one
+     850             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     851             : 
+     852             :   // sets the reference to the active tracker
+     853             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     854             : 
+     855             :   // sets the velocity reference to the active tracker
+     856             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     857             : 
+     858             :   // sets the reference trajectory to the active tracker
+     859             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     860             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     861             : 
+     862             :   // publishes
+     863             :   void publish(void);
+     864             : 
+     865             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     866             : 
+     867             :   double getMass(void);
+     868             : 
+     869             :   // publishes rviz-visualizable control reference
+     870             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     871             : 
+     872             :   void initializeControlOutput(void);
+     873             : 
+     874             :   // tell the mrs_odometry to disable its callbacks
+     875             :   void odometryCallbacksSrv(const bool input);
+     876             : 
+     877             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     878             : 
+     879             :   void                          setCallbacks(bool in);
+     880             :   bool                          isOffboard(void);
+     881             :   bool                          elandSrv(void);
+     882             :   std::tuple<bool, std::string> arming(const bool input);
+     883             : 
+     884             :   // safety functions impl
+     885             :   std::tuple<bool, std::string> ehover(void);
+     886             :   std::tuple<bool, std::string> hover(void);
+     887             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     888             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     889             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     890             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     891             :   std::tuple<bool, std::string> eland(void);
+     892             :   std::tuple<bool, std::string> failsafe(void);
+     893             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     894             : 
+     895             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     896             : };
+     897             : 
+     898             : //}
+     899             : 
+     900             : /* //{ onInit() */
+     901             : 
+     902         107 : void ControlManager::onInit() {
+     903         107 :   preinitialize();
+     904         107 : }
+     905             : 
+     906             : //}
+     907             : 
+     908             : /* preinitialize() //{ */
+     909             : 
+     910         107 : void ControlManager::preinitialize(void) {
+     911             : 
+     912         107 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     913             : 
+     914         107 :   ros::Time::waitForValid();
+     915             : 
+     916         107 :   mrs_lib::SubscribeHandlerOptions shopts;
+     917         107 :   shopts.nh                 = nh_;
+     918         107 :   shopts.node_name          = "ControlManager";
+     919         107 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     920         107 :   shopts.threadsafe         = true;
+     921         107 :   shopts.autostart          = true;
+     922         107 :   shopts.queue_size         = 10;
+     923         107 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     924             : 
+     925         107 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     926             : 
+     927         107 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     928         107 : }
+     929             : 
+     930             : //}
+     931             : 
+     932             : /* initialize() //{ */
+     933             : 
+     934         107 : void ControlManager::initialize(void) {
+     935             : 
+     936         107 :   joystick_start_press_time_      = ros::Time(0);
+     937         107 :   joystick_failsafe_press_time_   = ros::Time(0);
+     938         107 :   joystick_eland_press_time_      = ros::Time(0);
+     939         107 :   escalating_failsafe_time_       = ros::Time(0);
+     940         107 :   controller_tracker_switch_time_ = ros::Time(0);
+     941             : 
+     942         107 :   ROS_INFO("[ControlManager]: initializing");
+     943             : 
+     944             :   // --------------------------------------------------------------
+     945             :   // |         common handler for trackers and controllers        |
+     946             :   // --------------------------------------------------------------
+     947             : 
+     948         107 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     949             : 
+     950             :   // --------------------------------------------------------------
+     951             :   // |                           params                           |
+     952             :   // --------------------------------------------------------------
+     953             : 
+     954         214 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     955             : 
+     956         107 :   param_loader.loadParam("custom_config", _custom_config_);
+     957         107 :   param_loader.loadParam("platform_config", _platform_config_);
+     958         107 :   param_loader.loadParam("world_config", _world_config_);
+     959         107 :   param_loader.loadParam("network_config", _network_config_);
+     960             : 
+     961         107 :   if (_custom_config_ != "") {
+     962         107 :     param_loader.addYamlFile(_custom_config_);
+     963             :   }
+     964             : 
+     965         107 :   if (_platform_config_ != "") {
+     966         107 :     param_loader.addYamlFile(_platform_config_);
+     967             :   }
+     968             : 
+     969         107 :   if (_world_config_ != "") {
+     970         107 :     param_loader.addYamlFile(_world_config_);
+     971             :   }
+     972             : 
+     973         107 :   if (_network_config_ != "") {
+     974         107 :     param_loader.addYamlFile(_network_config_);
+     975             :   }
+     976             : 
+     977         107 :   param_loader.addYamlFileFromParam("private_config");
+     978         107 :   param_loader.addYamlFileFromParam("public_config");
+     979         107 :   param_loader.addYamlFileFromParam("private_trackers");
+     980         107 :   param_loader.addYamlFileFromParam("private_controllers");
+     981         107 :   param_loader.addYamlFileFromParam("public_controllers");
+     982             : 
+     983             :   // params passed from the launch file are not prefixed
+     984         107 :   param_loader.loadParam("uav_name", _uav_name_);
+     985         107 :   param_loader.loadParam("body_frame", _body_frame_);
+     986         107 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     987         107 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     988         107 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     989         107 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     990         107 :   param_loader.loadParam("g", common_handlers_->g);
+     991             : 
+     992             :   // motor params are also not prefixed, since they are common to more nodes
+     993         107 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+     994         107 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+     995         107 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+     996             : 
+     997             :   // | ----------------------- safety area ---------------------- |
+     998             : 
+     999             :   bool use_safety_area;
+    1000         107 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1001         107 :   use_safety_area_ = use_safety_area;
+    1002             : 
+    1003         107 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1004             : 
+    1005         107 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1006         107 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1007             : 
+    1008             :   {
+    1009             :     double temp;
+    1010         107 :     param_loader.loadParam("safety_area/vertical/min_z", temp);
+    1011             : 
+    1012         107 :     _safety_area_min_z_ = temp;
+    1013             :   }
+    1014             : 
+    1015         107 :   if (use_safety_area_) {
+    1016             : 
+    1017         255 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1018             : 
+    1019             :     try {
+    1020             : 
+    1021         170 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1022          85 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1023             : 
+    1024          85 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1025             :     }
+    1026             : 
+    1027           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1028           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1029           0 :       ros::shutdown();
+    1030             :     }
+    1031           0 :     catch (...) {
+    1032           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1033           0 :       ros::shutdown();
+    1034             :     }
+    1035             : 
+    1036          85 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1037             :   }
+    1038             : 
+    1039         107 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1040             : 
+    1041         107 :   param_loader.loadParam("state_input", _state_input_);
+    1042             : 
+    1043         107 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1044           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1045           0 :     ros::shutdown();
+    1046             :   }
+    1047             : 
+    1048         107 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1049         107 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1050         107 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1051             : 
+    1052         107 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1053         107 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1054         107 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1055         107 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1056         107 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1057             : 
+    1058         107 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1059         107 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1060         107 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1061         107 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1062         107 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1063         107 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1064         107 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1065         107 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1066             : 
+    1067         107 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1068         107 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1069             : 
+    1070         107 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1071             : 
+    1072         107 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1073           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1074           0 :     ros::shutdown();
+    1075             :   }
+    1076             : 
+    1077         107 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1078         107 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1079             : 
+    1080         107 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1081             : 
+    1082         107 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1083           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1084           0 :     ros::shutdown();
+    1085             :   }
+    1086             : 
+    1087         107 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1088         107 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1089             : 
+    1090         107 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1091             : 
+    1092         107 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1093           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1094           0 :     ros::shutdown();
+    1095             :   }
+    1096             : 
+    1097         107 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1098         107 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1099         107 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1100         107 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1101             : 
+    1102         107 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1103         107 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1104             : 
+    1105         107 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1106         107 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1107         107 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1108             : 
+    1109         107 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1110             : 
+    1111         107 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1112           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1113           0 :     ros::shutdown();
+    1114             :   }
+    1115             : 
+    1116             :   // default constraints
+    1117             : 
+    1118         107 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1119         107 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1120         107 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1121         107 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1122             : 
+    1123         107 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1124         107 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1125         107 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1126         107 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1127             : 
+    1128         107 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1129         107 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1130         107 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1131         107 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1132             : 
+    1133         107 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1134         107 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1135         107 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1136         107 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1137             : 
+    1138         107 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1139         107 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1140         107 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1141             : 
+    1142         107 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1143             : 
+    1144         107 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1145             : 
+    1146             :   // joystick
+    1147             : 
+    1148         107 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1149         107 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1150         107 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1151         107 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1152         107 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1153         107 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1154         107 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1155         107 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1156             : 
+    1157         107 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1158         107 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1159         107 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1160         107 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1161         107 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1162         107 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1163         107 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1164         107 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1165         107 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1166         107 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1167             : 
+    1168             :   // load channels
+    1169         107 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1170         107 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1171         107 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1172         107 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1173             : 
+    1174             :   // load channel multipliers
+    1175         107 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1176         107 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1177         107 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1178         107 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1179             : 
+    1180             :   bool bumper_enabled;
+    1181         107 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1182         107 :   bumper_enabled_ = bumper_enabled;
+    1183             : 
+    1184         107 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1185         107 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1186         107 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1187         107 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1188         107 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1189             : 
+    1190         107 :   param_loader.loadParam("obstacle_bumper/horizontal/min_distance_to_obstacle", _bumper_horizontal_distance_);
+    1191         107 :   param_loader.loadParam("obstacle_bumper/horizontal/derived_from_dynamics", _bumper_horizontal_derive_from_dynamics_);
+    1192             : 
+    1193         107 :   param_loader.loadParam("obstacle_bumper/vertical/min_distance_to_obstacle", _bumper_vertical_distance_);
+    1194         107 :   param_loader.loadParam("obstacle_bumper/vertical/derived_from_dynamics", _bumper_vertical_derive_from_dynamics_);
+    1195             : 
+    1196         107 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1197         107 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1198             : 
+    1199         107 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1200             : 
+    1201         107 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1202             : 
+    1203             :   // check the values of tracker error action
+    1204         107 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1205           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1206             :               EHOVER_STR);
+    1207           0 :     ros::shutdown();
+    1208             :   }
+    1209             : 
+    1210         107 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1211         107 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1212         107 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1213         107 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1214         107 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1215             : 
+    1216         107 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1217         107 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1218         107 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1219         107 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1220             : 
+    1221         107 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1222         107 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1223             : 
+    1224         107 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1225             : 
+    1226             :   // --------------------------------------------------------------
+    1227             :   // |             initialize the last control output             |
+    1228             :   // --------------------------------------------------------------
+    1229             : 
+    1230         107 :   initializeControlOutput();
+    1231             : 
+    1232             :   // | --------------------- tf transformer --------------------- |
+    1233             : 
+    1234         107 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1235         107 :   transformer_->setDefaultPrefix(_uav_name_);
+    1236         107 :   transformer_->retryLookupNewest(true);
+    1237             : 
+    1238             :   // | ------------------- scope timer logger ------------------- |
+    1239             : 
+    1240         107 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1241         321 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1242         107 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1243             : 
+    1244             :   // bind transformer to trackers and controllers for use
+    1245         107 :   common_handlers_->transformer = transformer_;
+    1246             : 
+    1247             :   // bind scope timer to trackers and controllers for use
+    1248         107 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1249         107 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1250             : 
+    1251         107 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1252         107 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1253         107 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1254         107 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1255         107 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1256             : 
+    1257         107 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1258             : 
+    1259         107 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1260             : 
+    1261         107 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1262             : 
+    1263         107 :   common_handlers_->uav_name = _uav_name_;
+    1264             : 
+    1265         107 :   common_handlers_->parent_nh = nh_;
+    1266             : 
+    1267             :   // --------------------------------------------------------------
+    1268             :   // |                        load trackers                       |
+    1269             :   // --------------------------------------------------------------
+    1270             : 
+    1271         214 :   std::vector<std::string> custom_trackers;
+    1272             : 
+    1273         107 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1274         107 :   param_loader.loadParam("trackers", custom_trackers);
+    1275             : 
+    1276         107 :   if (!custom_trackers.empty()) {
+    1277           1 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1278             :   }
+    1279             : 
+    1280         107 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1281         107 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1282             : 
+    1283         107 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1284             : 
+    1285         750 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1286             : 
+    1287        1286 :     std::string tracker_name = _tracker_names_.at(i);
+    1288             : 
+    1289             :     // load the controller parameters
+    1290        1286 :     std::string address;
+    1291        1286 :     std::string name_space;
+    1292             :     bool        human_switchable;
+    1293             : 
+    1294         643 :     param_loader.loadParam(tracker_name + "/address", address);
+    1295         643 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1296         643 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1297             : 
+    1298        1929 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1299         643 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1300             : 
+    1301             :     try {
+    1302         643 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1303         643 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1304             :     }
+    1305           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1306           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1307           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1308           0 :       ros::shutdown();
+    1309             :     }
+    1310           0 :     catch (pluginlib::PluginlibException& ex) {
+    1311           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1312           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1313           0 :       ros::shutdown();
+    1314             :     }
+    1315             :   }
+    1316             : 
+    1317         107 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1318             : 
+    1319         750 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1320             : 
+    1321         643 :     std::map<std::string, TrackerParams>::iterator it;
+    1322         643 :     it = trackers_.find(_tracker_names_.at(i));
+    1323             : 
+    1324             :     // create private handlers
+    1325             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1326        1286 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1327             : 
+    1328         643 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1329         643 :     private_handlers->name_space     = it->second.name_space;
+    1330         643 :     private_handlers->runtime_name   = _tracker_names_.at(i);
+    1331         643 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_.at(i));
+    1332             : 
+    1333         643 :     if (_custom_config_ != "") {
+    1334         643 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1335             :     }
+    1336             : 
+    1337         643 :     if (_platform_config_ != "") {
+    1338         643 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1339             :     }
+    1340             : 
+    1341         643 :     if (_world_config_ != "") {
+    1342         643 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1343             :     }
+    1344             : 
+    1345         643 :     if (_network_config_ != "") {
+    1346         643 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1347             :     }
+    1348             : 
+    1349         643 :     bool success = false;
+    1350             : 
+    1351             :     try {
+    1352         643 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1353         643 :       success = tracker_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1354             :     }
+    1355           0 :     catch (std::runtime_error& ex) {
+    1356           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1357             :     }
+    1358             : 
+    1359         643 :     if (!success) {
+    1360           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1361           0 :       ros::shutdown();
+    1362             :     }
+    1363             :   }
+    1364             : 
+    1365         107 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1366             : 
+    1367             :   // --------------------------------------------------------------
+    1368             :   // |           check the existance of selected trackers         |
+    1369             :   // --------------------------------------------------------------
+    1370             : 
+    1371             :   // | ------ check for the existance of the hover tracker ------ |
+    1372             : 
+    1373             :   // check if the hover_tracker is within the loaded trackers
+    1374             :   {
+    1375         107 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1376             : 
+    1377         107 :     if (idx) {
+    1378         107 :       _ehover_tracker_idx_ = idx.value();
+    1379             :     } else {
+    1380           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1381           0 :       ros::shutdown();
+    1382             :     }
+    1383             :   }
+    1384             : 
+    1385             :   // | ----- check for the existence of the landoff tracker ----- |
+    1386             : 
+    1387             :   {
+    1388         107 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1389             : 
+    1390         107 :     if (idx) {
+    1391         107 :       _landoff_tracker_idx_ = idx.value();
+    1392             :     } else {
+    1393           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1394           0 :       ros::shutdown();
+    1395             :     }
+    1396             :   }
+    1397             : 
+    1398             :   // | ------- check for the existence of the null tracker ------ |
+    1399             : 
+    1400             :   {
+    1401         107 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1402             : 
+    1403         107 :     if (idx) {
+    1404         107 :       _null_tracker_idx_ = idx.value();
+    1405             :     } else {
+    1406           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1407           0 :       ros::shutdown();
+    1408             :     }
+    1409             :   }
+    1410             : 
+    1411             :   // --------------------------------------------------------------
+    1412             :   // |         check existance of trackers for joystick           |
+    1413             :   // --------------------------------------------------------------
+    1414             : 
+    1415         107 :   if (_joystick_enabled_) {
+    1416             : 
+    1417         107 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1418             : 
+    1419         107 :     if (idx) {
+    1420         107 :       _joystick_tracker_idx_ = idx.value();
+    1421             :     } else {
+    1422           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1423           0 :       ros::shutdown();
+    1424             :     }
+    1425             :   }
+    1426             : 
+    1427         107 :   if (_bumper_switch_tracker_) {
+    1428             : 
+    1429         107 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1430             : 
+    1431         107 :     if (!idx) {
+    1432           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1433           0 :       ros::shutdown();
+    1434             :     }
+    1435             :   }
+    1436             : 
+    1437             :   {
+    1438         107 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1439             : 
+    1440         107 :     if (idx) {
+    1441         107 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1442             :     } else {
+    1443           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1444           0 :       ros::shutdown();
+    1445             :     }
+    1446             :   }
+    1447             : 
+    1448             :   // --------------------------------------------------------------
+    1449             :   // |                    load the controllers                    |
+    1450             :   // --------------------------------------------------------------
+    1451             : 
+    1452         214 :   std::vector<std::string> custom_controllers;
+    1453             : 
+    1454         107 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1455         107 :   param_loader.loadParam("controllers", custom_controllers);
+    1456             : 
+    1457         107 :   if (!custom_controllers.empty()) {
+    1458           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1459             :   }
+    1460             : 
+    1461         107 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1462             : 
+    1463             :   // for each controller in the list
+    1464         642 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1465             : 
+    1466        1070 :     std::string controller_name = _controller_names_.at(i);
+    1467             : 
+    1468             :     // load the controller parameters
+    1469        1070 :     std::string address;
+    1470        1070 :     std::string name_space;
+    1471             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1472             :     bool        human_switchable;
+    1473         535 :     param_loader.loadParam(controller_name + "/address", address);
+    1474         535 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1475         535 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1476         535 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1477         535 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1478         535 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1479             : 
+    1480             :     // check if the controller can output some of the required outputs
+    1481             :     {
+    1482             : 
+    1483         535 :       ControlOutputModalities_t outputs;
+    1484         535 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1485         535 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1486         535 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1487         535 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1488         535 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1489         535 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1490         535 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1491         535 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1492         535 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1493             : 
+    1494         535 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1495         535 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1496         535 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1497         535 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1498         535 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1499         535 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1500         535 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1501         535 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1502         535 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1503             : 
+    1504         520 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1505        1055 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1506             : 
+    1507         535 :       if (!meets_requirements) {
+    1508             : 
+    1509           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1510             :                   controller_name.c_str());
+    1511             : 
+    1512           0 :         if (_hw_api_inputs_.actuators) {
+    1513           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1514             :         }
+    1515             : 
+    1516           0 :         if (_hw_api_inputs_.control_group) {
+    1517           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1518             :         }
+    1519             : 
+    1520           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1521           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1522             :         }
+    1523             : 
+    1524           0 :         if (_hw_api_inputs_.attitude) {
+    1525           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1526             :         }
+    1527             : 
+    1528           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1529           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1530             :         }
+    1531             : 
+    1532           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1533           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1534             :         }
+    1535             : 
+    1536           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1537           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1538             :         }
+    1539             : 
+    1540           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1541           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1542             :         }
+    1543             : 
+    1544           0 :         if (_hw_api_inputs_.position) {
+    1545           0 :           ROS_ERROR("[ControlManager]: - position");
+    1546             :         }
+    1547             : 
+    1548           0 :         ros::shutdown();
+    1549             :       }
+    1550             : 
+    1551         535 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1552           0 :         ROS_ERROR(
+    1553             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1554           0 :         ros::shutdown();
+    1555             :       }
+    1556             :     }
+    1557             : 
+    1558             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1559             : 
+    1560         535 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1561             : 
+    1562         535 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1563          30 :       _safety_timer_rate_     = 200.0;
+    1564          30 :       desired_uav_state_rate_ = 250.0;
+    1565         505 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1566         415 :       _safety_timer_rate_     = 100.0;
+    1567         415 :       desired_uav_state_rate_ = 100.0;
+    1568          90 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1569          20 :       _safety_timer_rate_     = 30.0;
+    1570          20 :       _status_timer_rate_     = 1.0;
+    1571          20 :       desired_uav_state_rate_ = 40.0;
+    1572             : 
+    1573          20 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1574           4 :         _uav_state_max_missing_time_ = 0.2;
+    1575             :       }
+    1576          70 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1577          70 :       _safety_timer_rate_     = 20.0;
+    1578          70 :       _status_timer_rate_     = 1.0;
+    1579          70 :       desired_uav_state_rate_ = 20.0;
+    1580             : 
+    1581          70 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1582          14 :         _uav_state_max_missing_time_ = 1.0;
+    1583             :       }
+    1584             :     }
+    1585             : 
+    1586         535 :     if (eland_threshold == 0) {
+    1587         108 :       eland_threshold = 1e6;
+    1588             :     }
+    1589             : 
+    1590         535 :     if (failsafe_threshold == 0) {
+    1591         108 :       failsafe_threshold = 1e6;
+    1592             :     }
+    1593             : 
+    1594         535 :     if (odometry_innovation_threshold == 0) {
+    1595         109 :       odometry_innovation_threshold = 1e6;
+    1596             :     }
+    1597             : 
+    1598        1605 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1599         535 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1600             : 
+    1601             :     try {
+    1602         535 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1603         535 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1604             :     }
+    1605           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1606           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1607           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1608           0 :       ros::shutdown();
+    1609             :     }
+    1610           0 :     catch (pluginlib::PluginlibException& ex) {
+    1611           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1612           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1613           0 :       ros::shutdown();
+    1614             :     }
+    1615             :   }
+    1616             : 
+    1617         107 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1618             : 
+    1619         642 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1620             : 
+    1621         535 :     std::map<std::string, ControllerParams>::iterator it;
+    1622         535 :     it = controllers_.find(_controller_names_.at(i));
+    1623             : 
+    1624             :     // create private handlers
+    1625             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1626        1070 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1627             : 
+    1628         535 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1629         535 :     private_handlers->name_space     = it->second.name_space;
+    1630         535 :     private_handlers->runtime_name   = _controller_names_.at(i);
+    1631         535 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_.at(i));
+    1632             : 
+    1633         535 :     if (_custom_config_ != "") {
+    1634         535 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1635             :     }
+    1636             : 
+    1637         535 :     if (_platform_config_ != "") {
+    1638         535 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1639             :     }
+    1640             : 
+    1641         535 :     if (_world_config_ != "") {
+    1642         535 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1643             :     }
+    1644             : 
+    1645         535 :     if (_network_config_ != "") {
+    1646         535 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1647             :     }
+    1648             : 
+    1649         535 :     bool success = false;
+    1650             : 
+    1651             :     try {
+    1652             : 
+    1653         535 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1654         535 :       success = controller_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1655             :     }
+    1656           0 :     catch (std::runtime_error& ex) {
+    1657           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1658             :     }
+    1659             : 
+    1660         535 :     if (!success) {
+    1661           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1662           0 :       ros::shutdown();
+    1663             :     }
+    1664             :   }
+    1665             : 
+    1666         107 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1667             : 
+    1668             :   {
+    1669         107 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1670             : 
+    1671         107 :     if (idx) {
+    1672         107 :       _failsafe_controller_idx_ = idx.value();
+    1673             :     } else {
+    1674           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1675           0 :       ros::shutdown();
+    1676             :     }
+    1677             :   }
+    1678             : 
+    1679             :   {
+    1680         107 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1681             : 
+    1682         107 :     if (idx) {
+    1683         107 :       _eland_controller_idx_ = idx.value();
+    1684             :     } else {
+    1685           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1686           0 :       ros::shutdown();
+    1687             :     }
+    1688             :   }
+    1689             : 
+    1690             :   {
+    1691         107 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1692             : 
+    1693         107 :     if (idx) {
+    1694         107 :       _joystick_controller_idx_ = idx.value();
+    1695             :     } else {
+    1696           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1697           0 :       ros::shutdown();
+    1698             :     }
+    1699             :   }
+    1700             : 
+    1701         107 :   if (_bumper_switch_controller_) {
+    1702             : 
+    1703         107 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1704             : 
+    1705         107 :     if (!idx) {
+    1706           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1707           0 :       ros::shutdown();
+    1708             :     }
+    1709             :   }
+    1710             : 
+    1711             :   {
+    1712         107 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1713             : 
+    1714         107 :     if (idx) {
+    1715         107 :       _joystick_fallback_controller_idx_ = idx.value();
+    1716             :     } else {
+    1717           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1718           0 :       ros::shutdown();
+    1719             :     }
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                  activate the NullTracker                  |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726         107 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1727             : 
+    1728         107 :   tracker_list_.at(_null_tracker_idx_)->activate(last_tracker_cmd_);
+    1729         107 :   active_tracker_idx_ = _null_tracker_idx_;
+    1730             : 
+    1731             :   // --------------------------------------------------------------
+    1732             :   // |    activate the eland controller as the first controller   |
+    1733             :   // --------------------------------------------------------------
+    1734             : 
+    1735         107 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_.at(_eland_controller_idx_).c_str());
+    1736             : 
+    1737         107 :   controller_list_.at(_eland_controller_idx_)->activate(last_control_output_);
+    1738         107 :   active_controller_idx_ = _eland_controller_idx_;
+    1739             : 
+    1740             :   // update the time
+    1741             :   {
+    1742         214 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1743             : 
+    1744         107 :     controller_tracker_switch_time_ = ros::Time::now();
+    1745             :   }
+    1746             : 
+    1747         107 :   output_enabled_ = false;
+    1748             : 
+    1749             :   // | --------------- set the default constraints -------------- |
+    1750             : 
+    1751         107 :   sanitized_constraints_ = current_constraints_;
+    1752         107 :   setConstraints(current_constraints_);
+    1753             : 
+    1754             :   // | ------------------------ profiler ------------------------ |
+    1755             : 
+    1756         107 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1757             : 
+    1758             :   // | ----------------------- publishers ----------------------- |
+    1759             : 
+    1760         107 :   control_output_publisher_ = OutputPublisher(nh_);
+    1761             : 
+    1762         107 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1763         107 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1764         107 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1765         107 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1766         107 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1767         107 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1768         107 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1769         107 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1770         107 :   ph_mass_nominal_                       = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_nominal_out", 1, true);
+    1771         107 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1772         107 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1773         107 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1774         107 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1775         107 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1776         107 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1777         107 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1778         107 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1779         107 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1780         107 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1781         107 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1782             : 
+    1783             :   // | ------------------ publish nominal mass ------------------ |
+    1784             : 
+    1785             :   {
+    1786         107 :     std_msgs::Float64 nominal_mass;
+    1787             : 
+    1788         107 :     nominal_mass.data = _uav_mass_;
+    1789             : 
+    1790         107 :     ph_mass_nominal_.publish(nominal_mass);
+    1791             :   }
+    1792             : 
+    1793             :   // | ----------------------- subscribers ---------------------- |
+    1794             : 
+    1795         214 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1796         107 :   shopts.nh                 = nh_;
+    1797         107 :   shopts.node_name          = "ControlManager";
+    1798         107 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1799         107 :   shopts.threadsafe         = true;
+    1800         107 :   shopts.autostart          = true;
+    1801         107 :   shopts.queue_size         = 10;
+    1802         107 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1803             : 
+    1804         107 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1805         107 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1806           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1807           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1808             :   }
+    1809             : 
+    1810         107 :   if (_odometry_innovation_check_enabled_) {
+    1811         107 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1812             :   }
+    1813             : 
+    1814         107 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1815         107 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1816         107 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1817         107 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1818         107 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1819             : 
+    1820         107 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1821             : 
+    1822             :   // | -------------------- general services -------------------- |
+    1823             : 
+    1824         107 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1825         107 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1826         107 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1827         107 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1828         107 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1829         107 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1830         107 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1831         107 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1832         107 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1833         107 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1834         107 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1835         107 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1836         107 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1837         107 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1838         107 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1839         107 :   service_server_set_min_z_                  = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
+    1840         107 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1841         107 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1842         107 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1843         107 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1844         107 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1845         107 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1846         107 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1847         107 :   service_server_validate_reference_list_    = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+    1848         107 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1849         107 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1850         107 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1851         107 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1852             : 
+    1853         107 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1854         107 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1855         107 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1856         107 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1857         107 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1858         107 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1859             : 
+    1860             :   // | ---------------- setpoint command services --------------- |
+    1861             : 
+    1862             :   // human callable
+    1863         107 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1864         107 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1865         107 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1866         107 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1867         107 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1868         107 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1869             : 
+    1870         107 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1871         107 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1872             : 
+    1873         107 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1874             :   sh_velocity_reference_ =
+    1875         107 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1876             : 
+    1877         107 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1878             :   sh_trajectory_reference_ =
+    1879         107 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1880             : 
+    1881             :   // | --------------------- other services --------------------- |
+    1882             : 
+    1883         107 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1884         107 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1885             : 
+    1886             :   // | ------------------------- timers ------------------------- |
+    1887             : 
+    1888         107 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1889         107 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1890         107 :   timer_bumper_    = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerBumper, this);
+    1891         107 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1892         107 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1893         107 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1894         107 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1895             : 
+    1896             :   // | ----------------------- finish init ---------------------- |
+    1897             : 
+    1898         107 :   if (!param_loader.loadedSuccessfully()) {
+    1899           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1900           0 :     ros::shutdown();
+    1901             :   }
+    1902             : 
+    1903         107 :   is_initialized_ = true;
+    1904             : 
+    1905         107 :   ROS_INFO("[ControlManager]: initialized");
+    1906         107 : }
+    1907             : 
+    1908             : //}
+    1909             : 
+    1910             : // --------------------------------------------------------------
+    1911             : // |                           timers                           |
+    1912             : // --------------------------------------------------------------
+    1913             : 
+    1914             : /* timerHwApiCapabilities() //{ */
+    1915             : 
+    1916         182 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1917             : 
+    1918         364 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1919         364 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1920             : 
+    1921         182 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1922          75 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1923          75 :     return;
+    1924             :   }
+    1925             : 
+    1926         214 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1927             : 
+    1928         107 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1929             : 
+    1930         107 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1931           3 :     ROS_INFO("[ControlManager]: - actuator command");
+    1932           3 :     _hw_api_inputs_.actuators = true;
+    1933             :   }
+    1934             : 
+    1935         107 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1936           3 :     ROS_INFO("[ControlManager]: - control group command");
+    1937           3 :     _hw_api_inputs_.control_group = true;
+    1938             :   }
+    1939             : 
+    1940         107 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1941          80 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1942          80 :     _hw_api_inputs_.attitude_rate = true;
+    1943             :   }
+    1944             : 
+    1945         107 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1946          78 :     ROS_INFO("[ControlManager]: - attitude command");
+    1947          78 :     _hw_api_inputs_.attitude = true;
+    1948             :   }
+    1949             : 
+    1950         107 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1951           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1952           2 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1953             :   }
+    1954             : 
+    1955         107 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1956           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1957           2 :     _hw_api_inputs_.acceleration_hdg = true;
+    1958             :   }
+    1959             : 
+    1960         107 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1961           8 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1962           8 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1963             :   }
+    1964             : 
+    1965         107 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1966           4 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1967           4 :     _hw_api_inputs_.velocity_hdg = true;
+    1968             :   }
+    1969             : 
+    1970         107 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1971           2 :     ROS_INFO("[ControlManager]: - position command");
+    1972           2 :     _hw_api_inputs_.position = true;
+    1973             :   }
+    1974             : 
+    1975         107 :   initialize();
+    1976             : 
+    1977         107 :   timer_hw_api_capabilities_.stop();
+    1978             : }
+    1979             : 
+    1980             : //}
+    1981             : 
+    1982             : /* //{ timerStatus() */
+    1983             : 
+    1984       18762 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1985             : 
+    1986       18762 :   if (!is_initialized_) {
+    1987           0 :     return;
+    1988             :   }
+    1989             : 
+    1990       56286 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1991       56286 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1992             : 
+    1993             :   // copy member variables
+    1994       37524 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1995       37524 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1996       18762 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    1997       18762 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    1998       18762 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    1999       18762 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2000             : 
+    2001             :   double uav_x, uav_y, uav_z;
+    2002       18762 :   uav_x = uav_state.pose.position.x;
+    2003       18762 :   uav_y = uav_state.pose.position.y;
+    2004       18762 :   uav_z = uav_state.pose.position.z;
+    2005             : 
+    2006             :   // --------------------------------------------------------------
+    2007             :   // |                      print the status                      |
+    2008             :   // --------------------------------------------------------------
+    2009             : 
+    2010             :   {
+    2011       37524 :     std::string controller = _controller_names_.at(active_controller_idx);
+    2012       37524 :     std::string tracker    = _tracker_names_.at(active_tracker_idx);
+    2013       18762 :     double      mass       = last_control_output.diagnostics.total_mass;
+    2014       18762 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    2015       18762 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    2016       18762 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    2017       18762 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    2018             : 
+    2019       18762 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    2020             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2021             :   }
+    2022             : 
+    2023             :   // --------------------------------------------------------------
+    2024             :   // |                   publish the diagnostics                  |
+    2025             :   // --------------------------------------------------------------
+    2026             : 
+    2027       18762 :   publishDiagnostics();
+    2028             : 
+    2029             :   // --------------------------------------------------------------
+    2030             :   // |                publish if the offboard is on               |
+    2031             :   // --------------------------------------------------------------
+    2032             : 
+    2033       18762 :   if (offboard_mode_) {
+    2034             : 
+    2035       12796 :     std_msgs::Empty offboard_on_out;
+    2036             : 
+    2037       12796 :     ph_offboard_on_.publish(offboard_on_out);
+    2038             :   }
+    2039             : 
+    2040             :   // --------------------------------------------------------------
+    2041             :   // |                   publish the tilt error                   |
+    2042             :   // --------------------------------------------------------------
+    2043             :   {
+    2044       37524 :     std::scoped_lock lock(mutex_attitude_error_);
+    2045             : 
+    2046       18762 :     if (tilt_error_) {
+    2047             : 
+    2048       37524 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2049       18762 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2050       18762 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2051       18762 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2052             : 
+    2053       18762 :       ph_tilt_error_.publish(tilt_error_out);
+    2054             :     }
+    2055             :   }
+    2056             : 
+    2057             :   // --------------------------------------------------------------
+    2058             :   // |                  publish the control error                 |
+    2059             :   // --------------------------------------------------------------
+    2060             : 
+    2061       18762 :   if (position_error) {
+    2062             : 
+    2063       11694 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2064             : 
+    2065       23388 :     mrs_msgs::ControlError msg_out;
+    2066             : 
+    2067       11694 :     msg_out.header.stamp    = ros::Time::now();
+    2068       11694 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2069             : 
+    2070       11694 :     msg_out.position_errors.x    = pos_error_value(0);
+    2071       11694 :     msg_out.position_errors.y    = pos_error_value(1);
+    2072       11694 :     msg_out.position_errors.z    = pos_error_value(2);
+    2073       11694 :     msg_out.total_position_error = pos_error_value.norm();
+    2074             : 
+    2075       11694 :     if (yaw_error_) {
+    2076       11694 :       msg_out.yaw_error = yaw_error.value();
+    2077             :     }
+    2078             : 
+    2079       11694 :     std::map<std::string, ControllerParams>::iterator it;
+    2080             : 
+    2081       11694 :     it = controllers_.find(_controller_names_.at(active_controller_idx));
+    2082             : 
+    2083       11694 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2084       11694 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2085             : 
+    2086       11694 :     ph_control_error_.publish(msg_out);
+    2087             :   }
+    2088             : 
+    2089             :   // --------------------------------------------------------------
+    2090             :   // |                  publish the mass estimate                 |
+    2091             :   // --------------------------------------------------------------
+    2092             : 
+    2093       18762 :   if (last_control_output.diagnostics.mass_estimator) {
+    2094             : 
+    2095       10372 :     std_msgs::Float64 mass_estimate_out;
+    2096       10372 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2097             : 
+    2098       10372 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2099             :   }
+    2100             : 
+    2101             :   // --------------------------------------------------------------
+    2102             :   // |                 publish the current heading                |
+    2103             :   // --------------------------------------------------------------
+    2104             : 
+    2105       18762 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2106             : 
+    2107             :     try {
+    2108             : 
+    2109             :       double heading;
+    2110             : 
+    2111       15710 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2112             : 
+    2113       31420 :       mrs_msgs::Float64Stamped heading_out;
+    2114       15710 :       heading_out.header = uav_state.header;
+    2115       15710 :       heading_out.value  = heading;
+    2116             : 
+    2117       15710 :       ph_heading_.publish(heading_out);
+    2118             :     }
+    2119           0 :     catch (...) {
+    2120           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2121             :     }
+    2122             :   }
+    2123             : 
+    2124             :   // --------------------------------------------------------------
+    2125             :   // |                  publish the current speed                 |
+    2126             :   // --------------------------------------------------------------
+    2127             : 
+    2128       18762 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2129             : 
+    2130       15710 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2131             : 
+    2132       31420 :     mrs_msgs::Float64Stamped speed_out;
+    2133       15710 :     speed_out.header = uav_state.header;
+    2134       15710 :     speed_out.value  = speed;
+    2135             : 
+    2136       15710 :     ph_speed_.publish(speed_out);
+    2137             :   }
+    2138             : 
+    2139             :   // --------------------------------------------------------------
+    2140             :   // |               publish the safety area markers              |
+    2141             :   // --------------------------------------------------------------
+    2142             : 
+    2143       18762 :   if (use_safety_area_) {
+    2144             : 
+    2145       29920 :     mrs_msgs::ReferenceStamped temp_ref;
+    2146       14960 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2147             : 
+    2148       29920 :     geometry_msgs::TransformStamped tf;
+    2149             : 
+    2150       44880 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2151             : 
+    2152       14960 :     if (ret) {
+    2153             : 
+    2154       12242 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2155             : 
+    2156       24484 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2157       24484 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2158             : 
+    2159       24484 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2160             : 
+    2161       24484 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2162       24484 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2163             : 
+    2164       24484 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2165       24484 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2166             : 
+    2167             :       // if we fail in transforming the area at some point
+    2168             :       // do not publish it at all
+    2169       12242 :       bool tf_success = true;
+    2170             : 
+    2171       24484 :       geometry_msgs::TransformStamped tf = ret.value();
+    2172             : 
+    2173             :       /* transform area points to local origin //{ */
+    2174             : 
+    2175             :       // transform border bottom points to local origin
+    2176       61210 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2177             : 
+    2178       48968 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2179       48968 :         temp_ref.header.stamp         = ros::Time(0);
+    2180       48968 :         temp_ref.reference.position.x = border_points_bot_original.at(i).x;
+    2181       48968 :         temp_ref.reference.position.y = border_points_bot_original.at(i).y;
+    2182       48968 :         temp_ref.reference.position.z = border_points_bot_original.at(i).z;
+    2183             : 
+    2184       97936 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2185             : 
+    2186       48968 :           temp_ref = ret.value();
+    2187             : 
+    2188       48968 :           border_points_bot_transformed.at(i).x = temp_ref.reference.position.x;
+    2189       48968 :           border_points_bot_transformed.at(i).y = temp_ref.reference.position.y;
+    2190       48968 :           border_points_bot_transformed.at(i).z = temp_ref.reference.position.z;
+    2191             : 
+    2192             :         } else {
+    2193           0 :           tf_success = false;
+    2194             :         }
+    2195             :       }
+    2196             : 
+    2197             :       // transform border top points to local origin
+    2198       61210 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2199             : 
+    2200       48968 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2201       48968 :         temp_ref.header.stamp         = ros::Time(0);
+    2202       48968 :         temp_ref.reference.position.x = border_points_top_original.at(i).x;
+    2203       48968 :         temp_ref.reference.position.y = border_points_top_original.at(i).y;
+    2204       48968 :         temp_ref.reference.position.z = border_points_top_original.at(i).z;
+    2205             : 
+    2206       97936 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2207             : 
+    2208       48968 :           temp_ref = ret.value();
+    2209             : 
+    2210       48968 :           border_points_top_transformed.at(i).x = temp_ref.reference.position.x;
+    2211       48968 :           border_points_top_transformed.at(i).y = temp_ref.reference.position.y;
+    2212       48968 :           border_points_top_transformed.at(i).z = temp_ref.reference.position.z;
+    2213             : 
+    2214             :         } else {
+    2215           0 :           tf_success = false;
+    2216             :         }
+    2217             :       }
+    2218             : 
+    2219             :       //}
+    2220             : 
+    2221       24484 :       visualization_msgs::Marker safety_area_marker;
+    2222             : 
+    2223       12242 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2224       12242 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2225       12242 :       safety_area_marker.color.a         = 0.15;
+    2226       12242 :       safety_area_marker.scale.x         = 0.2;
+    2227       12242 :       safety_area_marker.color.r         = 1;
+    2228       12242 :       safety_area_marker.color.g         = 0;
+    2229       12242 :       safety_area_marker.color.b         = 0;
+    2230             : 
+    2231       12242 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2232             : 
+    2233       24484 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2234             : 
+    2235       12242 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2236       12242 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2237       12242 :       safety_area_coordinates_marker.color.a         = 1;
+    2238       12242 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2239       12242 :       safety_area_coordinates_marker.color.r         = 0;
+    2240       12242 :       safety_area_coordinates_marker.color.g         = 0;
+    2241       12242 :       safety_area_coordinates_marker.color.b         = 0;
+    2242             : 
+    2243       12242 :       safety_area_coordinates_marker.id = 0;
+    2244             : 
+    2245       12242 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2246             : 
+    2247             :       /* adding safety area points //{ */
+    2248             : 
+    2249             :       // bottom border
+    2250       61210 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2251             : 
+    2252       48968 :         safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+    2253       48968 :         safety_area_marker.points.push_back(border_points_bot_transformed.at((i + 1) % border_points_bot_transformed.size()));
+    2254             : 
+    2255       97936 :         std::stringstream ss;
+    2256             : 
+    2257       48968 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2258           0 :           ss << "idx: " << i << std::endl
+    2259           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+    2260           0 :              << "lon: " << border_points_bot_original.at(i).y;
+    2261             :         } else {
+    2262       48968 :           ss << "idx: " << i << std::endl
+    2263       48968 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+    2264       48968 :              << "y: " << border_points_bot_original.at(i).y;
+    2265             :         }
+    2266             : 
+    2267       48968 :         safety_area_coordinates_marker.color.r = 0;
+    2268       48968 :         safety_area_coordinates_marker.color.g = 0;
+    2269       48968 :         safety_area_coordinates_marker.color.b = 0;
+    2270             : 
+    2271       48968 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed.at(i);
+    2272       48968 :         safety_area_coordinates_marker.text          = ss.str();
+    2273       48968 :         safety_area_coordinates_marker.id++;
+    2274             : 
+    2275       48968 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2276             :       }
+    2277             : 
+    2278             :       // top border + top/bot edges
+    2279       61210 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2280             : 
+    2281       48968 :         safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+    2282       48968 :         safety_area_marker.points.push_back(border_points_top_transformed.at((i + 1) % border_points_top_transformed.size()));
+    2283             : 
+    2284       48968 :         safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+    2285       48968 :         safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+    2286             : 
+    2287       97936 :         std::stringstream ss;
+    2288             : 
+    2289       48968 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2290           0 :           ss << "idx: " << i << std::endl
+    2291           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+    2292           0 :              << "lon: " << border_points_bot_original.at(i).y;
+    2293             :         } else {
+    2294       48968 :           ss << "idx: " << i << std::endl
+    2295       48968 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+    2296       48968 :              << "y: " << border_points_bot_original.at(i).y;
+    2297             :         }
+    2298             : 
+    2299       48968 :         safety_area_coordinates_marker.color.r = 1;
+    2300       48968 :         safety_area_coordinates_marker.color.g = 1;
+    2301       48968 :         safety_area_coordinates_marker.color.b = 1;
+    2302             : 
+    2303       48968 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed.at(i);
+    2304       48968 :         safety_area_coordinates_marker.text          = ss.str();
+    2305       48968 :         safety_area_coordinates_marker.id++;
+    2306             : 
+    2307       48968 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2308             :       }
+    2309             : 
+    2310             :       //}
+    2311             : 
+    2312       12242 :       if (tf_success) {
+    2313             : 
+    2314       12242 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2315             : 
+    2316       12242 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2317             : 
+    2318       12242 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2319             :       }
+    2320             : 
+    2321             :     } else {
+    2322        2718 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2323             :     }
+    2324             :   }
+    2325             : 
+    2326             :   // --------------------------------------------------------------
+    2327             :   // |              publish the disturbances markers              |
+    2328             :   // --------------------------------------------------------------
+    2329             : 
+    2330       18762 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2331             : 
+    2332       20856 :     visualization_msgs::MarkerArray msg_out;
+    2333             : 
+    2334       10428 :     double id = 0;
+    2335             : 
+    2336       10428 :     double multiplier = 1.0;
+    2337             : 
+    2338       10428 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2339             : 
+    2340       10428 :     Eigen::Vector3d      vec3d;
+    2341       10428 :     geometry_msgs::Point point;
+    2342             : 
+    2343             :     /* world disturbance //{ */
+    2344             :     {
+    2345             : 
+    2346       20856 :       visualization_msgs::Marker marker;
+    2347             : 
+    2348       10428 :       marker.header.frame_id = uav_state.header.frame_id;
+    2349       10428 :       marker.header.stamp    = ros::Time::now();
+    2350       10428 :       marker.ns              = "control_manager";
+    2351       10428 :       marker.id              = id++;
+    2352       10428 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2353       10428 :       marker.action          = visualization_msgs::Marker::ADD;
+    2354             : 
+    2355             :       /* position //{ */
+    2356             : 
+    2357       10428 :       marker.pose.position.x = 0.0;
+    2358       10428 :       marker.pose.position.y = 0.0;
+    2359       10428 :       marker.pose.position.z = 0.0;
+    2360             : 
+    2361             :       //}
+    2362             : 
+    2363             :       /* orientation //{ */
+    2364             : 
+    2365       10428 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2366             : 
+    2367             :       //}
+    2368             : 
+    2369             :       /* origin //{ */
+    2370       10428 :       point.x = uav_x;
+    2371       10428 :       point.y = uav_y;
+    2372       10428 :       point.z = uav_z;
+    2373             : 
+    2374       10428 :       marker.points.push_back(point);
+    2375             : 
+    2376             :       //}
+    2377             : 
+    2378             :       /* tip //{ */
+    2379             : 
+    2380       10428 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2381       10428 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2382       10428 :       point.z = uav_z;
+    2383             : 
+    2384       10428 :       marker.points.push_back(point);
+    2385             : 
+    2386             :       //}
+    2387             : 
+    2388       10428 :       marker.scale.x = 0.05;
+    2389       10428 :       marker.scale.y = 0.05;
+    2390       10428 :       marker.scale.z = 0.05;
+    2391             : 
+    2392       10428 :       marker.color.a = 0.5;
+    2393       10428 :       marker.color.r = 1.0;
+    2394       10428 :       marker.color.g = 0.0;
+    2395       10428 :       marker.color.b = 0.0;
+    2396             : 
+    2397       10428 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2398             : 
+    2399       10428 :       msg_out.markers.push_back(marker);
+    2400             :     }
+    2401             : 
+    2402             :     //}
+    2403             : 
+    2404             :     /* body disturbance //{ */
+    2405             :     {
+    2406             : 
+    2407       20856 :       visualization_msgs::Marker marker;
+    2408             : 
+    2409       10428 :       marker.header.frame_id = uav_state.header.frame_id;
+    2410       10428 :       marker.header.stamp    = ros::Time::now();
+    2411       10428 :       marker.ns              = "control_manager";
+    2412       10428 :       marker.id              = id++;
+    2413       10428 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2414       10428 :       marker.action          = visualization_msgs::Marker::ADD;
+    2415             : 
+    2416             :       /* position //{ */
+    2417             : 
+    2418       10428 :       marker.pose.position.x = 0.0;
+    2419       10428 :       marker.pose.position.y = 0.0;
+    2420       10428 :       marker.pose.position.z = 0.0;
+    2421             : 
+    2422             :       //}
+    2423             : 
+    2424             :       /* orientation //{ */
+    2425             : 
+    2426       10428 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2427             : 
+    2428             :       //}
+    2429             : 
+    2430             :       /* origin //{ */
+    2431             : 
+    2432       10428 :       point.x = uav_x;
+    2433       10428 :       point.y = uav_y;
+    2434       10428 :       point.z = uav_z;
+    2435             : 
+    2436       10428 :       marker.points.push_back(point);
+    2437             : 
+    2438             :       //}
+    2439             : 
+    2440             :       /* tip //{ */
+    2441             : 
+    2442       10428 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2443       10428 :       vec3d = quat_eigen * vec3d;
+    2444             : 
+    2445       10428 :       point.x = uav_x + vec3d(0);
+    2446       10428 :       point.y = uav_y + vec3d(1);
+    2447       10428 :       point.z = uav_z + vec3d(2);
+    2448             : 
+    2449       10428 :       marker.points.push_back(point);
+    2450             : 
+    2451             :       //}
+    2452             : 
+    2453       10428 :       marker.scale.x = 0.05;
+    2454       10428 :       marker.scale.y = 0.05;
+    2455       10428 :       marker.scale.z = 0.05;
+    2456             : 
+    2457       10428 :       marker.color.a = 0.5;
+    2458       10428 :       marker.color.r = 0.0;
+    2459       10428 :       marker.color.g = 1.0;
+    2460       10428 :       marker.color.b = 0.0;
+    2461             : 
+    2462       10428 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2463             : 
+    2464       10428 :       msg_out.markers.push_back(marker);
+    2465             :     }
+    2466             : 
+    2467             :     //}
+    2468             : 
+    2469       10428 :     ph_disturbances_markers_.publish(msg_out);
+    2470             :   }
+    2471             : 
+    2472             :   // --------------------------------------------------------------
+    2473             :   // |               publish the current constraints              |
+    2474             :   // --------------------------------------------------------------
+    2475             : 
+    2476       18762 :   if (got_constraints_) {
+    2477             : 
+    2478       15628 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2479             : 
+    2480       15628 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2481             : 
+    2482       15628 :     ph_current_constraints_.publish(constraints);
+    2483             :   }
+    2484             : }
+    2485             : 
+    2486             : //}
+    2487             : 
+    2488             : /* //{ timerSafety() */
+    2489             : 
+    2490      339312 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2491             : 
+    2492      339312 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2493             : 
+    2494      339313 :   if (!is_initialized_) {
+    2495           0 :     return;
+    2496             :   }
+    2497             : 
+    2498      678626 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2499      678626 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2500             : 
+    2501             :   // copy member variables
+    2502      339313 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2503      339313 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2504      339313 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2505      339313 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2506      339313 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2507             : 
+    2508      644869 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2509      305556 :       active_tracker_idx == _null_tracker_idx_) {
+    2510      111586 :     return;
+    2511             :   }
+    2512             : 
+    2513      227727 :   if (odometry_switch_in_progress_) {
+    2514           5 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2515           5 :     return;
+    2516             :   }
+    2517             : 
+    2518             :   // | ------------------------ timeouts ------------------------ |
+    2519             : 
+    2520      227722 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2521      227722 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2522             : 
+    2523      227722 :     if (missing_for > _uav_state_max_missing_time_) {
+    2524           0 :       timeoutUavState(missing_for);
+    2525             :     }
+    2526             :   }
+    2527             : 
+    2528      227722 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2529           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2530             : 
+    2531           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2532           0 :       timeoutUavState(missing_for);
+    2533             :     }
+    2534             :   }
+    2535             : 
+    2536             :   // | -------------- eland and failsafe thresholds ------------- |
+    2537             : 
+    2538      227722 :   std::map<std::string, ControllerParams>::iterator it;
+    2539      227722 :   it = controllers_.find(_controller_names_.at(active_controller_idx));
+    2540             : 
+    2541      227722 :   _eland_threshold_               = it->second.eland_threshold;
+    2542      227722 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2543      227722 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2544             : 
+    2545             :   // | --------- calculate control errors and tilt angle -------- |
+    2546             : 
+    2547             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2548             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2549      227722 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2550         200 :     return;
+    2551             :   }
+    2552             : 
+    2553             :   {
+    2554      227522 :     std::scoped_lock lock(mutex_attitude_error_);
+    2555             : 
+    2556      227522 :     tilt_error_ = 0;
+    2557      227522 :     yaw_error_  = 0;
+    2558             :   }
+    2559             : 
+    2560             :   {
+    2561      227522 :     Eigen::Vector3d position_error = Eigen::Vector3d::Zero();
+    2562             : 
+    2563      227521 :     bool position_error_set = false;
+    2564             : 
+    2565      227521 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2566             : 
+    2567      226421 :       position_error(0) = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2568      226421 :       position_error(1) = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2569             : 
+    2570      226421 :       position_error_set = true;
+    2571             :     }
+    2572             : 
+    2573      227522 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2574             : 
+    2575      226421 :       position_error(2) = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2576             : 
+    2577      226421 :       position_error_set = true;
+    2578             :     }
+    2579             : 
+    2580      227522 :     if (position_error_set) {
+    2581             : 
+    2582      226421 :       mrs_lib::set_mutexed(mutex_position_error_, {position_error}, position_error_);
+    2583             :     }
+    2584             :   }
+    2585             : 
+    2586             :   // rotate the drone's z axis
+    2587      227522 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2588      227522 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2589             : 
+    2590             :   // calculate the angle between the drone's z axis and the world's z axis
+    2591      227522 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2592             : 
+    2593             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2594             : 
+    2595             :   // | --------------------- the tilt error --------------------- |
+    2596             : 
+    2597      227522 :   if (last_control_output.desired_orientation) {
+    2598             : 
+    2599             :     // calculate the desired drone's z axis in the world frame
+    2600      201597 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2601      201597 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2602             : 
+    2603             :     {
+    2604      201597 :       std::scoped_lock lock(mutex_attitude_error_);
+    2605             : 
+    2606             :       // calculate the angle between the drone's z axis and the world's z axis
+    2607      201597 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2608             : 
+    2609             :       // calculate the yaw error
+    2610      201597 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2611      201597 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2612             :     }
+    2613             :   }
+    2614             : 
+    2615      227522 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2616      227522 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2617             : 
+    2618             :   // --------------------------------------------------------------
+    2619             :   // |   activate the failsafe controller in case of large error  |
+    2620             :   // --------------------------------------------------------------
+    2621             : 
+    2622      227522 :   if (position_error) {
+    2623             : 
+    2624      226421 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2625             : 
+    2626          15 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2627             : 
+    2628          15 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2629             : 
+    2630           2 :         if (!failsafe_triggered_) {
+    2631             : 
+    2632           2 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2633             :                     _failsafe_threshold_, position_error.value()(0), position_error.value()(1), position_error.value()(2));
+    2634             : 
+    2635           2 :           failsafe();
+    2636             :         }
+    2637             :       }
+    2638             :     }
+    2639             :   }
+    2640             : 
+    2641             :   // --------------------------------------------------------------
+    2642             :   // |     activate emergency land in case of large innovation    |
+    2643             :   // --------------------------------------------------------------
+    2644             : 
+    2645      227521 :   if (_odometry_innovation_check_enabled_) {
+    2646             : 
+    2647      227520 :     std::optional<nav_msgs::Odometry::ConstPtr> innovation;
+    2648             : 
+    2649      227522 :     if (sh_odometry_innovation_.hasMsg()) {
+    2650      227522 :       innovation = {sh_odometry_innovation_.getMsg()};
+    2651             :     } else {
+    2652           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: missing estimator innnovation but the innovation check is enabled!");
+    2653             :     }
+    2654             : 
+    2655      227522 :     if (innovation) {
+    2656             : 
+    2657      227522 :       auto [x, y, z] = mrs_lib::getPosition(innovation.value());
+    2658             : 
+    2659      227521 :       double heading = 0;
+    2660             :       try {
+    2661      227521 :         heading = mrs_lib::getHeading(innovation.value());
+    2662             :       }
+    2663           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2664           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2665             :       }
+    2666             : 
+    2667      227521 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2668             : 
+    2669      227521 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2670             : 
+    2671           2 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2672             : 
+    2673           2 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2674             : 
+    2675           1 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2676             : 
+    2677           1 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2678             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2679             : 
+    2680           1 :             eland();
+    2681             :           }
+    2682             :         }
+    2683             :       }
+    2684             :     }
+    2685             :   }
+    2686             : 
+    2687             :   // --------------------------------------------------------------
+    2688             :   // |   activate emergency land in case of medium control error  |
+    2689             :   // --------------------------------------------------------------
+    2690             : 
+    2691             :   // | ------------------- tilt control error ------------------- |
+    2692             : 
+    2693      227521 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2694             : 
+    2695           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2696             : 
+    2697           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2698             : 
+    2699           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2700             : 
+    2701           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2702             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2703             : 
+    2704           0 :         eland();
+    2705             :       }
+    2706             :     }
+    2707             :   }
+    2708             : 
+    2709             :   // | ----------------- position control error ----------------- |
+    2710             : 
+    2711      227521 :   if (position_error) {
+    2712             : 
+    2713      226420 :     double error_size = position_error->norm();
+    2714             : 
+    2715      226421 :     if (error_size > _eland_threshold_ / 2.0) {
+    2716             : 
+    2717         671 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2718             : 
+    2719         671 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2720             : 
+    2721         470 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2722             : 
+    2723         114 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2724             : 
+    2725         114 :           ungripSrv();
+    2726             :         }
+    2727             :       }
+    2728             :     }
+    2729             : 
+    2730      226421 :     if (error_size > _eland_threshold_) {
+    2731             : 
+    2732         238 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2733             : 
+    2734         238 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2735             : 
+    2736          59 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2737             : 
+    2738           3 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2739             :                     position_error.value()(0), position_error.value()(1), position_error.value()(2));
+    2740             : 
+    2741           3 :           eland();
+    2742             :         }
+    2743             :       }
+    2744             :     }
+    2745             :   }
+    2746             : 
+    2747             :   // | -------------------- yaw control error ------------------- |
+    2748             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2749             : 
+    2750      227522 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2751             : 
+    2752      227521 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2753             : 
+    2754           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2755             : 
+    2756           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2757             : 
+    2758           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2759             : 
+    2760           0 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2761             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2762             : 
+    2763           0 :           ungripSrv();
+    2764             :         }
+    2765             :       }
+    2766             :     }
+    2767             : 
+    2768      227521 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2769             : 
+    2770           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2771             : 
+    2772           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2773             : 
+    2774           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2775             : 
+    2776           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2777             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2778             : 
+    2779           0 :           eland();
+    2780             :         }
+    2781             :       }
+    2782             :     }
+    2783             :   }
+    2784             : 
+    2785             :   // --------------------------------------------------------------
+    2786             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2787             :   // --------------------------------------------------------------
+    2788      227519 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2789             : 
+    2790           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_);
+    2791             : 
+    2792           0 :     arming(false);
+    2793             :   }
+    2794             : 
+    2795             :   // --------------------------------------------------------------
+    2796             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2797             :   // --------------------------------------------------------------
+    2798             : 
+    2799      227519 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2800             : 
+    2801      227521 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2802             : 
+    2803             :     // the time from the last controller/tracker switch
+    2804             :     // fyi: we should not
+    2805      227522 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2806             : 
+    2807             :     // if the tile error is over the threshold
+    2808             :     // && we are not ramping up during takeoff
+    2809      227522 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2810             : 
+    2811             :       // only account for the error if some time passed from the last tracker/controller switch
+    2812           0 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2813             : 
+    2814             :         // if the threshold was not exceeded before
+    2815           0 :         if (!tilt_error_disarm_over_thr_) {
+    2816             : 
+    2817           0 :           tilt_error_disarm_over_thr_ = true;
+    2818           0 :           tilt_error_disarm_time_     = ros::Time::now();
+    2819             : 
+    2820           0 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2821             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2822             : 
+    2823             :           // if it was exceeded before, just keep it
+    2824             :         } else {
+    2825             : 
+    2826           0 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2827             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2828             :         }
+    2829             : 
+    2830             :         // if the tile error is bad, but the controller just switched,
+    2831             :         // don't think its bad anymore
+    2832             :       } else {
+    2833             : 
+    2834           0 :         tilt_error_disarm_over_thr_ = false;
+    2835           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2836             :       }
+    2837             : 
+    2838             :       // if the tilt error is fine
+    2839             :     } else {
+    2840             : 
+    2841             :       // make it fine
+    2842      227522 :       tilt_error_disarm_over_thr_ = false;
+    2843      227522 :       tilt_error_disarm_time_     = ros::Time::now();
+    2844             :     }
+    2845             : 
+    2846             :     // calculate the time over the threshold
+    2847      227522 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2848             : 
+    2849             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2850      227522 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2851             : 
+    2852           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2853             : 
+    2854             :       // only when flying and not in failsafe
+    2855           0 :       if (is_flying) {
+    2856             : 
+    2857           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2858             : 
+    2859           0 :         toggleOutput(false);
+    2860           0 :         arming(false);
+    2861             :       }
+    2862             :     }
+    2863             :   }
+    2864             : 
+    2865             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2866             : 
+    2867             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2868      227520 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2869             : 
+    2870           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2871             : 
+    2872           0 :     toggleOutput(false);
+    2873             :   }
+    2874             : }  // namespace control_manager
+    2875             : 
+    2876             : //}
+    2877             : 
+    2878             : /* //{ timerEland() */
+    2879             : 
+    2880         446 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2881             : 
+    2882         446 :   if (!is_initialized_) {
+    2883          80 :     return;
+    2884             :   }
+    2885             : 
+    2886         892 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2887         892 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2888             : 
+    2889             :   // copy member variables
+    2890         446 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2891             : 
+    2892         446 :   if (!last_control_output.control_output) {
+    2893           0 :     return;
+    2894             :   }
+    2895             : 
+    2896         446 :   auto throttle = extractThrottle(last_control_output);
+    2897             : 
+    2898         446 :   if (!throttle) {
+    2899          80 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
+    2900          80 :     return;
+    2901             :   }
+    2902             : 
+    2903         366 :   if (current_state_landing_ == IDLE_STATE) {
+    2904             : 
+    2905           0 :     return;
+    2906             : 
+    2907         366 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2908             : 
+    2909             :     // --------------------------------------------------------------
+    2910             :     // |                            TODO                            |
+    2911             :     // This section needs work. The throttle landing detection      |
+    2912             :     // mechanism should be extracted and other mechanisms, such     |
+    2913             :     // as velocity-based detection should be used for high          |
+    2914             :     // modalities                                                   |
+    2915             :     // --------------------------------------------------------------
+    2916             : 
+    2917         366 :     if (!last_control_output.control_output) {
+    2918           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2919           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2920           0 :       return;
+    2921             :     }
+    2922             : 
+    2923             :     // recalculate the mass based on the throttle
+    2924         366 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2925         366 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2926             : 
+    2927             :     // condition for automatic motor turn off
+    2928         366 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2929          88 :       if (!throttle_under_threshold_) {
+    2930             : 
+    2931           4 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2932           4 :         throttle_under_threshold_          = true;
+    2933             :       }
+    2934             : 
+    2935          88 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2936             : 
+    2937             :     } else {
+    2938         278 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2939         278 :       throttle_under_threshold_          = false;
+    2940             :     }
+    2941             : 
+    2942         366 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2943             :       // enable callbacks? ... NO
+    2944             : 
+    2945           4 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2946           4 :       toggleOutput(false);
+    2947             : 
+    2948             :       // disarm the drone
+    2949           4 :       if (_eland_disarm_enabled_) {
+    2950             : 
+    2951           4 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2952           4 :         arming(false);
+    2953             :       }
+    2954             : 
+    2955           4 :       changeLandingState(IDLE_STATE);
+    2956             : 
+    2957           4 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2958             : 
+    2959           4 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2960           4 :       timer_eland_.stop();
+    2961           4 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2962             : 
+    2963             :       // we should NOT set eland_triggered_=true
+    2964             :     }
+    2965             :   }
+    2966             : }
+    2967             : 
+    2968             : //}
+    2969             : 
+    2970             : /* //{ timerFailsafe() */
+    2971             : 
+    2972       10191 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2973             : 
+    2974       10191 :   if (!is_initialized_) {
+    2975         479 :     return;
+    2976             :   }
+    2977             : 
+    2978       10191 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2979             : 
+    2980       20382 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2981       20382 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2982             : 
+    2983             :   // copy member variables
+    2984       10191 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2985             : 
+    2986       10191 :   updateControllers(uav_state);
+    2987             : 
+    2988       10191 :   publish();
+    2989             : 
+    2990       10191 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2991             : 
+    2992       10191 :   if (!last_control_output.control_output) {
+    2993           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    2994           0 :     return;
+    2995             :   }
+    2996             : 
+    2997       10191 :   auto throttle = extractThrottle(last_control_output);
+    2998             : 
+    2999       10191 :   if (!throttle) {
+    3000         479 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    3001         479 :     return;
+    3002             :   }
+    3003             : 
+    3004             :   // --------------------------------------------------------------
+    3005             :   // |                            TODO                            |
+    3006             :   // This section needs work. The throttle landing detection      |
+    3007             :   // mechanism should be extracted and other mechanisms, such     |
+    3008             :   // as velocity-based detection should be used for high          |
+    3009             :   // modalities                                                   |
+    3010             :   // --------------------------------------------------------------
+    3011             : 
+    3012        9712 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    3013        9712 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    3014             : 
+    3015             :   // condition for automatic motor turn off
+    3016        9712 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    3017             : 
+    3018        1414 :     if (!throttle_under_threshold_) {
+    3019             : 
+    3020           7 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    3021           7 :       throttle_under_threshold_          = true;
+    3022             :     }
+    3023             : 
+    3024        1414 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    3025             : 
+    3026             :   } else {
+    3027             : 
+    3028        8298 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    3029        8298 :     throttle_under_threshold_          = false;
+    3030             :   }
+    3031             : 
+    3032             :   // condition for automatic motor turn off
+    3033        9712 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3034             : 
+    3035           7 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3036             : 
+    3037           7 :     arming(false);
+    3038             :   }
+    3039             : }
+    3040             : 
+    3041             : //}
+    3042             : 
+    3043             : /* //{ timerJoystick() */
+    3044             : 
+    3045       66016 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3046             : 
+    3047       66016 :   if (!is_initialized_) {
+    3048           0 :     return;
+    3049             :   }
+    3050             : 
+    3051      198048 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3052      198048 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3053             : 
+    3054      132032 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3055             : 
+    3056             :   // if start was pressed and held for > 3.0 s
+    3057       66016 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3058             : 
+    3059           0 :     joystick_start_press_time_ = ros::Time(0);
+    3060             : 
+    3061           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3062             :              _joystick_controller_name_.c_str());
+    3063             : 
+    3064           0 :     joystick_start_pressed_ = false;
+    3065             : 
+    3066           0 :     switchTracker(_joystick_tracker_name_);
+    3067           0 :     switchController(_joystick_controller_name_);
+    3068             :   }
+    3069             : 
+    3070             :   // if RT+LT were pressed and held for > 0.1 s
+    3071       66016 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3072             : 
+    3073           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3074             : 
+    3075           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3076             : 
+    3077           0 :     joystick_failsafe_pressed_ = false;
+    3078             : 
+    3079           0 :     failsafe();
+    3080             :   }
+    3081             : 
+    3082             :   // if joypads were pressed and held for > 0.1 s
+    3083       66016 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3084             : 
+    3085           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3086             : 
+    3087           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3088             : 
+    3089           0 :     joystick_failsafe_pressed_ = false;
+    3090             : 
+    3091           0 :     eland();
+    3092             :   }
+    3093             : 
+    3094             :   // if back was pressed and held for > 0.1 s
+    3095       66016 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3096             : 
+    3097           0 :     joystick_back_press_time_ = ros::Time(0);
+    3098             : 
+    3099             :     // activate/deactivate the joystick goto functionality
+    3100           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3101             : 
+    3102           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3103             :   }
+    3104             : 
+    3105             :   // if the GOTO functionality is enabled...
+    3106       66016 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3107             : 
+    3108           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3109             : 
+    3110             :     // create the reference
+    3111             : 
+    3112           0 :     mrs_msgs::Vec4::Request request;
+    3113             : 
+    3114           0 :     if (fabs(joystick_data->axes.at(_channel_pitch_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_roll_)) >= 0.05 ||
+    3115           0 :         fabs(joystick_data->axes.at(_channel_heading_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_throttle_)) >= 0.05) {
+    3116             : 
+    3117           0 :       if (_joystick_mode_ == 0) {
+    3118             : 
+    3119           0 :         request.goal.at(REF_X)       = _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * _joystick_carrot_distance_;
+    3120           0 :         request.goal.at(REF_Y)       = _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * _joystick_carrot_distance_;
+    3121           0 :         request.goal.at(REF_Z)       = _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_);
+    3122           0 :         request.goal.at(REF_HEADING) = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+    3123             : 
+    3124           0 :         mrs_msgs::Vec4::Response response;
+    3125             : 
+    3126           0 :         callbackGotoFcu(request, response);
+    3127             : 
+    3128           0 :       } else if (_joystick_mode_ == 1) {
+    3129             : 
+    3130           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3131             : 
+    3132           0 :         double dt = 0.2;
+    3133             : 
+    3134           0 :         trajectory.fly_now         = true;
+    3135           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3136           0 :         trajectory.use_heading     = true;
+    3137           0 :         trajectory.dt              = dt;
+    3138             : 
+    3139           0 :         mrs_msgs::Reference point;
+    3140           0 :         point.position.x = 0;
+    3141           0 :         point.position.y = 0;
+    3142           0 :         point.position.z = 0;
+    3143           0 :         point.heading    = 0;
+    3144             : 
+    3145           0 :         trajectory.points.push_back(point);
+    3146             : 
+    3147           0 :         double speed = 1.0;
+    3148             : 
+    3149           0 :         for (int i = 0; i < 50; i++) {
+    3150             : 
+    3151           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * (speed * dt);
+    3152           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * (speed * dt);
+    3153           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_) * (speed * dt);
+    3154           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+    3155             : 
+    3156           0 :           trajectory.points.push_back(point);
+    3157             :         }
+    3158             : 
+    3159           0 :         setTrajectoryReference(trajectory);
+    3160             :       }
+    3161             :     }
+    3162             :   }
+    3163             : 
+    3164       66016 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3165             : 
+    3166             :     // create the reference
+    3167        1420 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3168             : 
+    3169         710 :     double des_x       = 0;
+    3170         710 :     double des_y       = 0;
+    3171         710 :     double des_z       = 0;
+    3172         710 :     double des_heading = 0;
+    3173             : 
+    3174         710 :     bool nothing_to_do = true;
+    3175             : 
+    3176             :     // copy member variables
+    3177        1420 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3178             : 
+    3179         710 :     if (rc_channels->channels.size() < 4) {
+    3180             : 
+    3181           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3182             :                          int(rc_channels->channels.size()));
+    3183           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3184             : 
+    3185             :     } else {
+    3186             : 
+    3187         710 :       double tmp_x       = RCChannelToRange(rc_channels->channels.at(_rc_channel_pitch_), _rc_horizontal_speed_, 0.1);
+    3188         710 :       double tmp_y       = -RCChannelToRange(rc_channels->channels.at(_rc_channel_roll_), _rc_horizontal_speed_, 0.1);
+    3189         710 :       double tmp_z       = RCChannelToRange(rc_channels->channels.at(_rc_channel_throttle_), _rc_vertical_speed_, 0.3);
+    3190         710 :       double tmp_heading = -RCChannelToRange(rc_channels->channels.at(_rc_channel_heading_), _rc_heading_rate_, 0.1);
+    3191             : 
+    3192         710 :       if (abs(tmp_x) > 1e-3) {
+    3193         254 :         des_x         = tmp_x;
+    3194         254 :         nothing_to_do = false;
+    3195             :       }
+    3196             : 
+    3197         710 :       if (abs(tmp_y) > 1e-3) {
+    3198         113 :         des_y         = tmp_y;
+    3199         113 :         nothing_to_do = false;
+    3200             :       }
+    3201             : 
+    3202         710 :       if (abs(tmp_z) > 1e-3) {
+    3203         228 :         des_z         = tmp_z;
+    3204         228 :         nothing_to_do = false;
+    3205             :       }
+    3206             : 
+    3207         710 :       if (abs(tmp_heading) > 1e-3) {
+    3208          48 :         des_heading   = tmp_heading;
+    3209          48 :         nothing_to_do = false;
+    3210             :       }
+    3211             :     }
+    3212             : 
+    3213         710 :     if (!nothing_to_do) {
+    3214             : 
+    3215         643 :       request.reference.header.frame_id = "fcu_untilted";
+    3216             : 
+    3217         643 :       request.reference.reference.use_heading_rate = true;
+    3218             : 
+    3219         643 :       request.reference.reference.velocity.x   = des_x;
+    3220         643 :       request.reference.reference.velocity.y   = des_y;
+    3221         643 :       request.reference.reference.velocity.z   = des_z;
+    3222         643 :       request.reference.reference.heading_rate = des_heading;
+    3223             : 
+    3224        1286 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3225             : 
+    3226             :       // disable callbacks of all trackers
+    3227         643 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3228             : 
+    3229             :       // enable the callbacks for the active tracker
+    3230         643 :       req_enable_callbacks.data = true;
+    3231             :       {
+    3232         643 :         std::scoped_lock lock(mutex_tracker_list_);
+    3233             : 
+    3234         643 :         tracker_list_.at(active_tracker_idx_)
+    3235         643 :             ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3236             :       }
+    3237             : 
+    3238         643 :       callbacks_enabled_ = true;
+    3239             : 
+    3240         643 :       callbackVelocityReferenceService(request, response);
+    3241             : 
+    3242         643 :       callbacks_enabled_ = false;
+    3243             : 
+    3244         643 :       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);
+    3245             : 
+    3246             :       // disable the callbacks back again
+    3247         643 :       req_enable_callbacks.data = false;
+    3248             :       {
+    3249         643 :         std::scoped_lock lock(mutex_tracker_list_);
+    3250             : 
+    3251         643 :         tracker_list_.at(active_tracker_idx_)
+    3252         643 :             ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3253             :       }
+    3254             :     }
+    3255             :   }
+    3256             : }
+    3257             : 
+    3258             : //}
+    3259             : 
+    3260             : /* //{ timerBumper() */
+    3261             : 
+    3262        2396 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3263             : 
+    3264        2396 :   if (!is_initialized_) {
+    3265        2137 :     return;
+    3266             :   }
+    3267             : 
+    3268        4792 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3269        4792 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3270             : 
+    3271             :   // | ---------------------- preconditions --------------------- |
+    3272             : 
+    3273        2396 :   if (!sh_bumper_.hasMsg()) {
+    3274        2133 :     return;
+    3275             :   }
+    3276             : 
+    3277         263 :   if (!bumper_enabled_) {
+    3278           0 :     return;
+    3279             :   }
+    3280             : 
+    3281         263 :   if (!isFlyingNormally() && !bumper_repulsing_) {
+    3282           4 :     return;
+    3283             :   }
+    3284             : 
+    3285         259 :   if (!got_uav_state_) {
+    3286           0 :     return;
+    3287             :   }
+    3288             : 
+    3289         259 :   if (sh_bumper_.hasMsg() && (ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3290             : 
+    3291           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: obstacle bumper is missing messages although we got them before");
+    3292             : 
+    3293           0 :     return;
+    3294             :   }
+    3295             : 
+    3296         259 :   timer_bumper_.setPeriod(ros::Duration(1.0 / _bumper_timer_rate_));
+    3297             : 
+    3298             :   // | ------------------ call the bumper logic ----------------- |
+    3299             : 
+    3300         259 :   bumperPushFromObstacle();
+    3301             : }
+    3302             : 
+    3303             : //}
+    3304             : 
+    3305             : /* //{ timerPirouette() */
+    3306             : 
+    3307           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3308             : 
+    3309           0 :   if (!is_initialized_) {
+    3310           0 :     return;
+    3311             :   }
+    3312             : 
+    3313           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3314           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3315             : 
+    3316           0 :   pirouette_iterator_++;
+    3317             : 
+    3318           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3319           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3320           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3321             : 
+    3322           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3323             : 
+    3324           0 :     _pirouette_enabled_ = false;
+    3325           0 :     timer_pirouette_.stop();
+    3326             : 
+    3327           0 :     setCallbacks(true);
+    3328             : 
+    3329           0 :     return;
+    3330             :   }
+    3331             : 
+    3332             :   // set the reference
+    3333           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3334             : 
+    3335           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3336             : 
+    3337           0 :   reference_request.header.frame_id      = "";
+    3338           0 :   reference_request.header.stamp         = ros::Time(0);
+    3339           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3340           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3341           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3342           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3343             : 
+    3344             :   // enable the callbacks for the active tracker
+    3345             :   {
+    3346           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3347             : 
+    3348           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3349           0 :     req_enable_callbacks.data = true;
+    3350             : 
+    3351           0 :     tracker_list_.at(active_tracker_idx_)
+    3352           0 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3353             : 
+    3354           0 :     callbacks_enabled_ = true;
+    3355             :   }
+    3356             : 
+    3357           0 :   setReference(reference_request);
+    3358             : 
+    3359             :   {
+    3360           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3361             : 
+    3362             :     // disable the callbacks for the active tracker
+    3363           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3364           0 :     req_enable_callbacks.data = false;
+    3365             : 
+    3366           0 :     tracker_list_.at(active_tracker_idx_)
+    3367           0 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3368             : 
+    3369           0 :     callbacks_enabled_ = false;
+    3370             :   }
+    3371             : }
+    3372             : 
+    3373             : //}
+    3374             : 
+    3375             : // --------------------------------------------------------------
+    3376             : // |                           asyncs                           |
+    3377             : // --------------------------------------------------------------
+    3378             : 
+    3379             : /* asyncControl() //{ */
+    3380             : 
+    3381      145544 : void ControlManager::asyncControl(void) {
+    3382             : 
+    3383      145544 :   if (!is_initialized_) {
+    3384           0 :     return;
+    3385             :   }
+    3386             : 
+    3387      291088 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3388             : 
+    3389      436632 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3390      436632 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3391             : 
+    3392             :   // copy member variables
+    3393      291088 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3394      145544 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3395             : 
+    3396      145544 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3397             : 
+    3398             :     // run the safety timer
+    3399             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3400      136033 :     while (running_safety_timer_) {
+    3401             : 
+    3402        1080 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3403        1080 :       ros::Duration wait(0.001);
+    3404        1080 :       wait.sleep();
+    3405             : 
+    3406        1080 :       if (!running_safety_timer_) {
+    3407        1074 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3408        1074 :         break;
+    3409             :       }
+    3410             :     }
+    3411             : 
+    3412      136027 :     ros::TimerEvent safety_timer_event;
+    3413      136027 :     timerSafety(safety_timer_event);
+    3414             : 
+    3415      136027 :     updateTrackers();
+    3416             : 
+    3417      136027 :     updateControllers(uav_state);
+    3418             : 
+    3419      136027 :     if (got_constraints_) {
+    3420             : 
+    3421             :       // update the constraints to trackers, if need to
+    3422      135312 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3423             : 
+    3424      135312 :       if (enforce && !constraints_being_enforced_) {
+    3425             : 
+    3426          82 :         setConstraintsToTrackers(enforce.value());
+    3427          82 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3428             : 
+    3429          82 :         constraints_being_enforced_ = true;
+    3430             : 
+    3431          82 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3432             : 
+    3433          82 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3434             :                           _controller_names_.at(active_controller_idx).c_str());
+    3435             : 
+    3436      135230 :       } else if (!enforce && constraints_being_enforced_) {
+    3437             : 
+    3438          76 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3439             : 
+    3440          76 :         constraints_being_enforced_ = false;
+    3441             : 
+    3442          76 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3443             : 
+    3444          76 :         setConstraintsToTrackers(current_constraints);
+    3445             :       }
+    3446             :     }
+    3447             : 
+    3448      136027 :     publish();
+    3449             :   }
+    3450             : 
+    3451             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3452      145544 :   if (odometry_switch_in_progress_) {
+    3453             : 
+    3454           5 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3455           5 :     timer_safety_.start();
+    3456           5 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3457           5 :     odometry_switch_in_progress_ = false;
+    3458             : 
+    3459             :     {
+    3460          10 :       std::scoped_lock lock(mutex_uav_state_);
+    3461             : 
+    3462           5 :       ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    3463             :                uav_state.pose.position.z, uav_heading_);
+    3464             :     }
+    3465             :   }
+    3466             : }
+    3467             : 
+    3468             : //}
+    3469             : 
+    3470             : // --------------------------------------------------------------
+    3471             : // |                          callbacks                         |
+    3472             : // --------------------------------------------------------------
+    3473             : 
+    3474             : // | --------------------- topic callbacks -------------------- |
+    3475             : 
+    3476             : /* //{ callbackOdometry() */
+    3477             : 
+    3478           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3479             : 
+    3480           0 :   if (!is_initialized_) {
+    3481           0 :     return;
+    3482             :   }
+    3483             : 
+    3484           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3485           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3486             : 
+    3487           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3488             : 
+    3489             :   // | ------------------ check for time stamp ------------------ |
+    3490             : 
+    3491             :   {
+    3492           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3493             : 
+    3494           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3495           0 :       return;
+    3496             :     }
+    3497             :   }
+    3498             : 
+    3499             :   // | --------------------- check for nans --------------------- |
+    3500             : 
+    3501           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3502           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3503           0 :     return;
+    3504             :   }
+    3505             : 
+    3506             :   // | ---------------------- frame switch ---------------------- |
+    3507             : 
+    3508             :   /* Odometry frame switch //{ */
+    3509             : 
+    3510             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3511             : 
+    3512           0 :   mrs_msgs::UavState uav_state_odom;
+    3513             : 
+    3514           0 :   uav_state_odom.header   = odom->header;
+    3515           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3516           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3517             : 
+    3518             :   // | ----- check for change in odometry frame of reference ---- |
+    3519             : 
+    3520           0 :   if (got_uav_state_) {
+    3521             : 
+    3522           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3523             : 
+    3524           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3525             :       {
+    3526           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3527             : 
+    3528           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,
+    3529             :                  uav_state_.pose.position.z, uav_heading_);
+    3530             :       }
+    3531             : 
+    3532           0 :       odometry_switch_in_progress_ = true;
+    3533             : 
+    3534             :       // we have to stop safety timer, otherwise it will interfere
+    3535           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3536           0 :       timer_safety_.stop();
+    3537           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3538             : 
+    3539             :       // wait for the safety timer to stop if its running
+    3540           0 :       while (running_safety_timer_) {
+    3541             : 
+    3542           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3543           0 :         ros::Duration wait(0.001);
+    3544           0 :         wait.sleep();
+    3545             : 
+    3546           0 :         if (!running_safety_timer_) {
+    3547           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3548           0 :           break;
+    3549             :         }
+    3550             :       }
+    3551             : 
+    3552             :       // we have to also for the oneshot control timer to finish
+    3553           0 :       while (running_async_control_) {
+    3554             : 
+    3555           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3556           0 :         ros::Duration wait(0.001);
+    3557           0 :         wait.sleep();
+    3558             : 
+    3559           0 :         if (!running_async_control_) {
+    3560           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3561           0 :           break;
+    3562             :         }
+    3563             :       }
+    3564             : 
+    3565             :       {
+    3566           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3567             : 
+    3568           0 :         tracker_list_.at(active_tracker_idx_)->switchOdometrySource(uav_state_odom);
+    3569           0 :         controller_list_.at(active_controller_idx_)->switchOdometrySource(uav_state_odom);
+    3570             :       }
+    3571             :     }
+    3572             :   }
+    3573             : 
+    3574             :   //}
+    3575             : 
+    3576             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3577             : 
+    3578             :   {
+    3579           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3580             : 
+    3581           0 :     previous_uav_state_ = uav_state_;
+    3582             : 
+    3583           0 :     uav_state_ = mrs_msgs::UavState();
+    3584             : 
+    3585           0 :     uav_state_.header           = odom->header;
+    3586           0 :     uav_state_.pose             = odom->pose.pose;
+    3587           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3588             : 
+    3589             :     // transform the twist into the header's frame
+    3590             :     {
+    3591             :       // the velocity from the odometry
+    3592           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3593           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3594           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3595           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3596           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3597           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3598             : 
+    3599           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3600             : 
+    3601           0 :       if (res) {
+    3602           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3603           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3604           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3605             :       } else {
+    3606           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3607             :                            odom->header.frame_id.c_str());
+    3608           0 :         return;
+    3609             :       }
+    3610             :     }
+    3611             : 
+    3612             :     // calculate the euler angles
+    3613           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3614             : 
+    3615             :     try {
+    3616           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3617             :     }
+    3618           0 :     catch (...) {
+    3619           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3620             :     }
+    3621             : 
+    3622           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3623             : 
+    3624           0 :     got_uav_state_ = true;
+    3625             :   }
+    3626             : 
+    3627             :   // run the control loop asynchronously in an OneShotTimer
+    3628             :   // but only if its not already running
+    3629           0 :   if (!running_async_control_) {
+    3630             : 
+    3631           0 :     running_async_control_ = true;
+    3632             : 
+    3633           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3634             :   }
+    3635             : }
+    3636             : 
+    3637             : //}
+    3638             : 
+    3639             : /* //{ callbackUavState() */
+    3640             : 
+    3641      170192 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3642             : 
+    3643      170192 :   if (!is_initialized_) {
+    3644       22186 :     return;
+    3645             :   }
+    3646             : 
+    3647      340384 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3648      340384 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3649             : 
+    3650      170192 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3651             : 
+    3652             :   // | ------------------ check for time stamp ------------------ |
+    3653             : 
+    3654             :   {
+    3655      170192 :     std::scoped_lock lock(mutex_uav_state_);
+    3656             : 
+    3657      170192 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3658       22186 :       return;
+    3659             :     }
+    3660             :   }
+    3661             : 
+    3662             :   // | --------------------- check for nans --------------------- |
+    3663             : 
+    3664      148006 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3665           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3666           0 :     return;
+    3667             :   }
+    3668             : 
+    3669             :   // | -------------------- check for hiccups ------------------- |
+    3670             : 
+    3671             :   /* hickup detection //{ */
+    3672             : 
+    3673      148006 :   double alpha               = 0.99;
+    3674      148006 :   double alpha2              = 0.666;
+    3675      148006 :   double uav_state_count_lim = 1000;
+    3676             : 
+    3677      148006 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3678             : 
+    3679             :   // belive only reasonable numbers
+    3680      148006 :   if (uav_state_dt <= 1.0) {
+    3681             : 
+    3682      147792 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3683             : 
+    3684      147792 :     if (uav_state_count_ < uav_state_count_lim) {
+    3685       81073 :       uav_state_count_++;
+    3686             :     }
+    3687             :   }
+    3688             : 
+    3689      148006 :   if (uav_state_count_ == uav_state_count_lim) {
+    3690             : 
+    3691             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3692             : 
+    3693       66782 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3694             : 
+    3695       22994 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3696             : 
+    3697       43788 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3698             : 
+    3699       43788 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3700             :     }
+    3701             : 
+    3702       66782 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3703             : 
+    3704             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3705             : 
+    3706           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3707           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3708           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3709           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3710           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3711           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3712           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3713           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3714           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3715           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3716             :     }
+    3717             :   }
+    3718             : 
+    3719             :   //}
+    3720             : 
+    3721             :   // | ---------------------- frame switch ---------------------- |
+    3722             : 
+    3723             :   /* frame switch //{ */
+    3724             : 
+    3725             :   // | ----- check for change in odometry frame of reference ---- |
+    3726             : 
+    3727      148006 :   if (got_uav_state_) {
+    3728             : 
+    3729      147899 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3730             : 
+    3731           5 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3732             :       {
+    3733          10 :         std::scoped_lock lock(mutex_uav_state_);
+    3734             : 
+    3735           5 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3736             :                  uav_state_.pose.position.z, uav_heading_);
+    3737             :       }
+    3738             : 
+    3739           5 :       odometry_switch_in_progress_ = true;
+    3740             : 
+    3741             :       // we have to stop safety timer, otherwise it will interfere
+    3742           5 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3743           5 :       timer_safety_.stop();
+    3744           5 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3745             : 
+    3746             :       // wait for the safety timer to stop if its running
+    3747           5 :       while (running_safety_timer_) {
+    3748             : 
+    3749           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3750           0 :         ros::Duration wait(0.001);
+    3751           0 :         wait.sleep();
+    3752             : 
+    3753           0 :         if (!running_safety_timer_) {
+    3754           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3755           0 :           break;
+    3756             :         }
+    3757             :       }
+    3758             : 
+    3759             :       // we have to also for the oneshot control timer to finish
+    3760           5 :       while (running_async_control_) {
+    3761             : 
+    3762           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3763           0 :         ros::Duration wait(0.001);
+    3764           0 :         wait.sleep();
+    3765             : 
+    3766           0 :         if (!running_async_control_) {
+    3767           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3768           0 :           break;
+    3769             :         }
+    3770             :       }
+    3771             : 
+    3772             :       {
+    3773          10 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3774             : 
+    3775           5 :         tracker_list_.at(active_tracker_idx_)->switchOdometrySource(*uav_state);
+    3776           5 :         controller_list_.at(active_controller_idx_)->switchOdometrySource(*uav_state);
+    3777             :       }
+    3778             :     }
+    3779             :   }
+    3780             : 
+    3781             :   //}
+    3782             : 
+    3783             :   // --------------------------------------------------------------
+    3784             :   // |           copy the UavState message for later use          |
+    3785             :   // --------------------------------------------------------------
+    3786             : 
+    3787             :   {
+    3788      148006 :     std::scoped_lock lock(mutex_uav_state_);
+    3789             : 
+    3790      148006 :     previous_uav_state_ = uav_state_;
+    3791             : 
+    3792      148006 :     uav_state_ = *uav_state;
+    3793             : 
+    3794      148006 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3795             : 
+    3796             :     try {
+    3797      148006 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3798             :     }
+    3799           0 :     catch (...) {
+    3800           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3801             :     }
+    3802             : 
+    3803      148006 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3804             : 
+    3805      148006 :     got_uav_state_ = true;
+    3806             :   }
+    3807             : 
+    3808             :   // run the control loop asynchronously in an OneShotTimer
+    3809             :   // but only if its not already running
+    3810      148006 :   if (!running_async_control_) {
+    3811             : 
+    3812      145544 :     running_async_control_ = true;
+    3813             : 
+    3814      145544 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3815             :   }
+    3816             : }
+    3817             : 
+    3818             : //}
+    3819             : 
+    3820             : /* //{ callbackGNSS() */
+    3821             : 
+    3822       75687 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3823             : 
+    3824       75687 :   if (!is_initialized_) {
+    3825          87 :     return;
+    3826             :   }
+    3827             : 
+    3828      226800 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3829      226800 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3830             : 
+    3831       75600 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3832             : }
+    3833             : 
+    3834             : //}
+    3835             : 
+    3836             : /* callbackJoystick() //{ */
+    3837             : 
+    3838           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3839             : 
+    3840           0 :   if (!is_initialized_) {
+    3841           0 :     return;
+    3842             :   }
+    3843             : 
+    3844           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3845           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3846             : 
+    3847             :   // copy member variables
+    3848           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3849           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3850             : 
+    3851           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3852             : 
+    3853             :   // TODO check if the array is smaller than the largest idx
+    3854           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3855           0 :     return;
+    3856             :   }
+    3857             : 
+    3858             :   // | ---- switching back to fallback tracker and controller --- |
+    3859             : 
+    3860             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3861           0 :   if ((joystick_data->buttons.at(_channel_A_) == 1 || joystick_data->buttons.at(_channel_B_) == 1 || joystick_data->buttons.at(_channel_X_) == 1 ||
+    3862           0 :        joystick_data->buttons.at(_channel_Y_) == 1) &&
+    3863           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3864             : 
+    3865           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3866             : 
+    3867           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3868           0 :     switchController(_joystick_fallback_controller_name_);
+    3869             : 
+    3870           0 :     joystick_goto_enabled_ = false;
+    3871             :   }
+    3872             : 
+    3873             :   // | ------- joystick control activation ------- |
+    3874             : 
+    3875             :   // if start button was pressed
+    3876           0 :   if (joystick_data->buttons.at(_channel_start_) == 1) {
+    3877             : 
+    3878           0 :     if (!joystick_start_pressed_) {
+    3879             : 
+    3880           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3881             : 
+    3882           0 :       joystick_start_pressed_    = true;
+    3883           0 :       joystick_start_press_time_ = ros::Time::now();
+    3884             :     }
+    3885             : 
+    3886           0 :   } else if (joystick_start_pressed_) {
+    3887             : 
+    3888           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3889             : 
+    3890           0 :     joystick_start_pressed_    = false;
+    3891           0 :     joystick_start_press_time_ = ros::Time(0);
+    3892             :   }
+    3893             : 
+    3894             :   // | ---------------- Joystick goto activation ---------------- |
+    3895             : 
+    3896             :   // if back button was pressed
+    3897           0 :   if (joystick_data->buttons.at(_channel_back_) == 1) {
+    3898             : 
+    3899           0 :     if (!joystick_back_pressed_) {
+    3900             : 
+    3901           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3902             : 
+    3903           0 :       joystick_back_pressed_    = true;
+    3904           0 :       joystick_back_press_time_ = ros::Time::now();
+    3905             :     }
+    3906             : 
+    3907           0 :   } else if (joystick_back_pressed_) {
+    3908             : 
+    3909           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3910             : 
+    3911           0 :     joystick_back_pressed_    = false;
+    3912           0 :     joystick_back_press_time_ = ros::Time(0);
+    3913             :   }
+    3914             : 
+    3915             :   // | ------------------------ Failsafes ----------------------- |
+    3916             : 
+    3917             :   // if LT and RT buttons are both pressed down
+    3918           0 :   if (joystick_data->axes.at(_channel_LT_) < -0.99 && joystick_data->axes.at(_channel_RT_) < -0.99) {
+    3919             : 
+    3920           0 :     if (!joystick_failsafe_pressed_) {
+    3921             : 
+    3922           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3923             : 
+    3924           0 :       joystick_failsafe_pressed_    = true;
+    3925           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3926             :     }
+    3927             : 
+    3928           0 :   } else if (joystick_failsafe_pressed_) {
+    3929             : 
+    3930           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3931             : 
+    3932           0 :     joystick_failsafe_pressed_    = false;
+    3933           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3934             :   }
+    3935             : 
+    3936             :   // if left and right joypads are both pressed down
+    3937           0 :   if (joystick_data->buttons.at(_channel_L_joy_) == 1 && joystick_data->buttons.at(_channel_R_joy_) == 1) {
+    3938             : 
+    3939           0 :     if (!joystick_eland_pressed_) {
+    3940             : 
+    3941           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3942             : 
+    3943           0 :       joystick_eland_pressed_    = true;
+    3944           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3945             :     }
+    3946             : 
+    3947           0 :   } else if (joystick_eland_pressed_) {
+    3948             : 
+    3949           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3950             : 
+    3951           0 :     joystick_eland_pressed_    = false;
+    3952           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3953             :   }
+    3954             : }
+    3955             : 
+    3956             : //}
+    3957             : 
+    3958             : /* //{ callbackHwApiStatus() */
+    3959             : 
+    3960      438762 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3961             : 
+    3962      438762 :   if (!is_initialized_) {
+    3963         440 :     return;
+    3964             :   }
+    3965             : 
+    3966     1314966 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3967     1314966 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3968             : 
+    3969      876644 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3970             : 
+    3971             :   // | ------ detect and print the changes in offboard mode ----- |
+    3972      438322 :   if (state->offboard) {
+    3973             : 
+    3974      306199 :     if (!offboard_mode_) {
+    3975         101 :       offboard_mode_          = true;
+    3976         101 :       offboard_mode_was_true_ = true;
+    3977         101 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3978             :     }
+    3979             : 
+    3980             :   } else {
+    3981             : 
+    3982      132123 :     if (offboard_mode_) {
+    3983           0 :       offboard_mode_ = false;
+    3984           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3985             :     }
+    3986             :   }
+    3987             : 
+    3988             :   // | --------- detect and print the changes in arming --------- |
+    3989      438322 :   if (state->armed == true) {
+    3990             : 
+    3991      319552 :     if (!armed_) {
+    3992         106 :       armed_ = true;
+    3993         106 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    3994             :     }
+    3995             : 
+    3996             :   } else {
+    3997             : 
+    3998      118770 :     if (armed_) {
+    3999          20 :       armed_ = false;
+    4000          20 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    4001             :     }
+    4002             :   }
+    4003             : }
+    4004             : 
+    4005             : //}
+    4006             : 
+    4007             : /* //{ callbackRC() */
+    4008             : 
+    4009       24316 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    4010             : 
+    4011       24316 :   if (!is_initialized_) {
+    4012           0 :     return;
+    4013             :   }
+    4014             : 
+    4015       72948 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    4016       72948 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    4017             : 
+    4018       48632 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    4019             : 
+    4020       24316 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    4021             : 
+    4022             :   // | ------------------- rc joystic control ------------------- |
+    4023             : 
+    4024             :   // when the switch change its position
+    4025       24316 :   if (_rc_goto_enabled_) {
+    4026             : 
+    4027       24316 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    4028             : 
+    4029           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    4030             :                          int(rc->channels.size()));
+    4031             : 
+    4032             :     } else {
+    4033             : 
+    4034       24316 :       bool channel_low  = rc->channels.at(_rc_joystick_channel_) < (0.5 - RC_DEADBAND) ? true : false;
+    4035       24316 :       bool channel_high = rc->channels.at(_rc_joystick_channel_) > (0.5 + RC_DEADBAND) ? true : false;
+    4036             : 
+    4037       24316 :       if (channel_low) {
+    4038       21989 :         rc_joystick_channel_was_low_ = true;
+    4039             :       }
+    4040             : 
+    4041             :       // rc control activation
+    4042       24316 :       if (!rc_goto_active_) {
+    4043             : 
+    4044       21990 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    4045             : 
+    4046           2 :           if (isFlyingNormally()) {
+    4047             : 
+    4048           2 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    4049             : 
+    4050           2 :             callbacks_enabled_ = false;
+    4051             : 
+    4052           2 :             std_srvs::SetBoolRequest req_goto_out;
+    4053           2 :             req_goto_out.data = false;
+    4054             : 
+    4055           2 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4056           2 :             req_enable_callbacks.data = callbacks_enabled_;
+    4057             : 
+    4058             :             {
+    4059           4 :               std::scoped_lock lock(mutex_tracker_list_);
+    4060             : 
+    4061             :               // disable callbacks of all trackers
+    4062          14 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4063          12 :                 tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4064             :               }
+    4065             :             }
+    4066             : 
+    4067           2 :             rc_goto_active_ = true;
+    4068             : 
+    4069             :           } else {
+    4070             : 
+    4071           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4072           2 :           }
+    4073             : 
+    4074       21988 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4075             : 
+    4076           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4077             :         }
+    4078             :       }
+    4079             : 
+    4080             :       // rc control deactivation
+    4081       24316 :       if (rc_goto_active_ && channel_low) {
+    4082             : 
+    4083           1 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4084             : 
+    4085           1 :         callbacks_enabled_ = true;
+    4086             : 
+    4087           1 :         std_srvs::SetBoolRequest req_goto_out;
+    4088           1 :         req_goto_out.data = true;
+    4089             : 
+    4090           1 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4091           1 :         req_enable_callbacks.data = callbacks_enabled_;
+    4092             : 
+    4093             :         {
+    4094           2 :           std::scoped_lock lock(mutex_tracker_list_);
+    4095             : 
+    4096             :           // enable callbacks of all trackers
+    4097           7 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4098           6 :             tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4099             :           }
+    4100             :         }
+    4101             : 
+    4102           1 :         rc_goto_active_ = false;
+    4103             :       }
+    4104             : 
+    4105             :       // do not forget to update the last... variable
+    4106             :       // only do that if its out of the deadband
+    4107       24316 :       if (channel_high || channel_low) {
+    4108       24316 :         rc_joystick_channel_last_value_ = rc->channels.at(_rc_joystick_channel_);
+    4109             :       }
+    4110             :     }
+    4111             :   }
+    4112             : 
+    4113             :   // | ----------------- RC escalating failsafe ----------------- |
+    4114             : 
+    4115       24316 :   if (_rc_escalating_failsafe_enabled_) {
+    4116             : 
+    4117       24316 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4118             : 
+    4119           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4120             :                          int(rc->channels.size()));
+    4121             : 
+    4122             :     } else {
+    4123             : 
+    4124       24316 :       if (rc->channels.at(_rc_escalating_failsafe_channel_) >= _rc_escalating_failsafe_threshold_) {
+    4125             : 
+    4126         144 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4127             : 
+    4128         288 :         auto [success, message] = escalatingFailsafe();
+    4129             : 
+    4130         144 :         if (success) {
+    4131           3 :           rc_escalating_failsafe_triggered_ = true;
+    4132             :         }
+    4133             :       }
+    4134             :     }
+    4135             :   }
+    4136             : }
+    4137             : 
+    4138             : //}
+    4139             : 
+    4140             : // | --------------------- topic timeouts --------------------- |
+    4141             : 
+    4142             : /* timeoutUavState() //{ */
+    4143             : 
+    4144           0 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4145             : 
+    4146           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4147             : 
+    4148           0 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4149             : 
+    4150             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4151             :     // in place of the callbackUavState/callbackOdometry().
+    4152             : 
+    4153           0 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4154             : 
+    4155           0 :     failsafe();
+    4156             :   }
+    4157           0 : }
+    4158             : 
+    4159             : //}
+    4160             : 
+    4161             : // | -------------------- service callbacks ------------------- |
+    4162             : 
+    4163             : /* //{ callbackSwitchTracker() */
+    4164             : 
+    4165         210 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4166             : 
+    4167         210 :   if (!is_initialized_) {
+    4168           0 :     return false;
+    4169             :   }
+    4170             : 
+    4171         210 :   if (failsafe_triggered_ || eland_triggered_) {
+    4172             : 
+    4173           0 :     std::stringstream ss;
+    4174           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4175             : 
+    4176           0 :     res.message = ss.str();
+    4177           0 :     res.success = false;
+    4178             : 
+    4179           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4180             : 
+    4181           0 :     return true;
+    4182             :   }
+    4183             : 
+    4184         210 :   auto [success, response] = switchTracker(req.value);
+    4185             : 
+    4186         210 :   res.success = success;
+    4187         210 :   res.message = response;
+    4188             : 
+    4189         210 :   return true;
+    4190             : }
+    4191             : 
+    4192             : //}
+    4193             : 
+    4194             : /* callbackSwitchController() //{ */
+    4195             : 
+    4196         211 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4197             : 
+    4198         211 :   if (!is_initialized_) {
+    4199           0 :     return false;
+    4200             :   }
+    4201             : 
+    4202         211 :   if (failsafe_triggered_ || eland_triggered_) {
+    4203             : 
+    4204           0 :     std::stringstream ss;
+    4205           0 :     ss << "can not switch controller, eland or failsafe active";
+    4206             : 
+    4207           0 :     res.message = ss.str();
+    4208           0 :     res.success = false;
+    4209             : 
+    4210           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4211             : 
+    4212           0 :     return true;
+    4213             :   }
+    4214             : 
+    4215         211 :   auto [success, response] = switchController(req.value);
+    4216             : 
+    4217         211 :   res.success = success;
+    4218         211 :   res.message = response;
+    4219             : 
+    4220         211 :   return true;
+    4221             : }
+    4222             : 
+    4223             : //}
+    4224             : 
+    4225             : /* //{ callbackSwitchTracker() */
+    4226             : 
+    4227           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4228             : 
+    4229           0 :   if (!is_initialized_) {
+    4230           0 :     return false;
+    4231             :   }
+    4232             : 
+    4233           0 :   std::stringstream message;
+    4234             : 
+    4235           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4236             : 
+    4237           0 :     message << "can not reset tracker, eland or failsafe active";
+    4238             : 
+    4239           0 :     res.message = message.str();
+    4240           0 :     res.success = false;
+    4241             : 
+    4242           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4243             : 
+    4244           0 :     return true;
+    4245             :   }
+    4246             : 
+    4247             :   // reactivate the current tracker
+    4248             :   {
+    4249           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4250             : 
+    4251           0 :     std::string tracker_name = _tracker_names_.at(active_tracker_idx_);
+    4252             : 
+    4253           0 :     bool succ = tracker_list_.at(active_tracker_idx_)->resetStatic();
+    4254             : 
+    4255           0 :     if (succ) {
+    4256           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4257           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4258             :     } else {
+    4259           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4260           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4261             :     }
+    4262             :   }
+    4263             : 
+    4264           0 :   res.message = message.str();
+    4265           0 :   res.success = true;
+    4266             : 
+    4267           0 :   return true;
+    4268             : }
+    4269             : 
+    4270             : //}
+    4271             : 
+    4272             : /* //{ callbackEHover() */
+    4273             : 
+    4274           2 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4275             : 
+    4276           2 :   if (!is_initialized_) {
+    4277           0 :     return false;
+    4278             :   }
+    4279             : 
+    4280           2 :   if (failsafe_triggered_ || eland_triggered_) {
+    4281             : 
+    4282           0 :     std::stringstream ss;
+    4283           0 :     ss << "can not switch controller, eland or failsafe active";
+    4284             : 
+    4285           0 :     res.message = ss.str();
+    4286           0 :     res.success = false;
+    4287             : 
+    4288           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4289             : 
+    4290           0 :     return true;
+    4291             :   }
+    4292             : 
+    4293           2 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4294             : 
+    4295           2 :   auto [success, message] = ehover();
+    4296             : 
+    4297           2 :   res.success = success;
+    4298           2 :   res.message = message;
+    4299             : 
+    4300           2 :   return true;
+    4301             : }
+    4302             : 
+    4303             : //}
+    4304             : 
+    4305             : /* callbackFailsafe() //{ */
+    4306             : 
+    4307           4 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4308             : 
+    4309           4 :   if (!is_initialized_) {
+    4310           0 :     return false;
+    4311             :   }
+    4312             : 
+    4313           4 :   if (failsafe_triggered_) {
+    4314             : 
+    4315           0 :     std::stringstream ss;
+    4316           0 :     ss << "can not activate failsafe, it is already active";
+    4317             : 
+    4318           0 :     res.message = ss.str();
+    4319           0 :     res.success = false;
+    4320             : 
+    4321           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4322             : 
+    4323           0 :     return true;
+    4324             :   }
+    4325             : 
+    4326           4 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4327             : 
+    4328           4 :   auto [success, message] = failsafe();
+    4329             : 
+    4330           4 :   res.success = success;
+    4331           4 :   res.message = message;
+    4332             : 
+    4333           4 :   return true;
+    4334             : }
+    4335             : 
+    4336             : //}
+    4337             : 
+    4338             : /* callbackFailsafeEscalating() //{ */
+    4339             : 
+    4340           7 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4341             : 
+    4342           7 :   if (!is_initialized_) {
+    4343           0 :     return false;
+    4344             :   }
+    4345             : 
+    4346           7 :   if (_service_escalating_failsafe_enabled_) {
+    4347             : 
+    4348           7 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4349             : 
+    4350          14 :     auto [success, message] = escalatingFailsafe();
+    4351             : 
+    4352           7 :     res.success = success;
+    4353           7 :     res.message = message;
+    4354             : 
+    4355             :   } else {
+    4356             : 
+    4357           0 :     std::stringstream ss;
+    4358           0 :     ss << "escalating failsafe is disabled";
+    4359             : 
+    4360           0 :     res.success = false;
+    4361           0 :     res.message = ss.str();
+    4362             : 
+    4363           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4364             :   }
+    4365             : 
+    4366           7 :   return true;
+    4367             : }
+    4368             : 
+    4369             : //}
+    4370             : 
+    4371             : /* //{ callbackELand() */
+    4372             : 
+    4373           2 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4374             : 
+    4375           2 :   if (!is_initialized_) {
+    4376           0 :     return false;
+    4377             :   }
+    4378             : 
+    4379           2 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4380             : 
+    4381           2 :   auto [success, message] = eland();
+    4382             : 
+    4383           2 :   res.success = success;
+    4384           2 :   res.message = message;
+    4385             : 
+    4386           2 :   return true;
+    4387             : }
+    4388             : 
+    4389             : //}
+    4390             : 
+    4391             : /* //{ callbackParachute() */
+    4392             : 
+    4393           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4394             : 
+    4395           0 :   if (!is_initialized_) {
+    4396           0 :     return false;
+    4397             :   }
+    4398             : 
+    4399           0 :   if (!_parachute_enabled_) {
+    4400             : 
+    4401           0 :     std::stringstream ss;
+    4402           0 :     ss << "parachute disabled";
+    4403           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4404           0 :     res.message = ss.str();
+    4405           0 :     res.success = false;
+    4406             :   }
+    4407             : 
+    4408           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4409             : 
+    4410           0 :   auto [success, message] = deployParachute();
+    4411             : 
+    4412           0 :   res.success = success;
+    4413           0 :   res.message = message;
+    4414             : 
+    4415           0 :   return true;
+    4416             : }
+    4417             : 
+    4418             : //}
+    4419             : 
+    4420             : /* //{ callbackSetMinZ() */
+    4421             : 
+    4422           0 : bool ControlManager::callbackSetMinZ([[maybe_unused]] mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res) {
+    4423             : 
+    4424           0 :   if (!is_initialized_) {
+    4425           0 :     return false;
+    4426             :   }
+    4427             : 
+    4428           0 :   if (!use_safety_area_) {
+    4429           0 :     res.success = true;
+    4430           0 :     res.message = "safety area is disabled";
+    4431           0 :     return true;
+    4432             :   }
+    4433             : 
+    4434             :   // | -------- transform min_z to the safety area frame -------- |
+    4435             : 
+    4436           0 :   mrs_msgs::ReferenceStamped point;
+    4437           0 :   point.header               = req.header;
+    4438           0 :   point.reference.position.z = req.value;
+    4439             : 
+    4440           0 :   auto result = transformer_->transformSingle(point, _safety_area_vertical_frame_);
+    4441             : 
+    4442           0 :   if (result) {
+    4443             : 
+    4444           0 :     _safety_area_min_z_ = result.value().reference.position.z;
+    4445             : 
+    4446           0 :     res.success = true;
+    4447           0 :     res.message = "safety area's min z updated";
+    4448             : 
+    4449             :   } else {
+    4450             : 
+    4451           0 :     res.success = false;
+    4452           0 :     res.message = "could not transform the value to safety area's vertical frame";
+    4453             :   }
+    4454             : 
+    4455           0 :   return true;
+    4456             : }
+    4457             : 
+    4458             : //}
+    4459             : 
+    4460             : /* //{ callbackToggleOutput() */
+    4461             : 
+    4462         108 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4463             : 
+    4464         108 :   if (!is_initialized_) {
+    4465           0 :     return false;
+    4466             :   }
+    4467             : 
+    4468         108 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4469             : 
+    4470             :   // copy member variables
+    4471         216 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4472             : 
+    4473         216 :   std::stringstream ss;
+    4474             : 
+    4475         108 :   bool prereq_check = true;
+    4476             : 
+    4477             :   {
+    4478         216 :     mrs_msgs::ReferenceStamped current_coord;
+    4479         108 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4480         108 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4481         108 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4482             : 
+    4483         108 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4484           1 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4485           1 :       prereq_check = false;
+    4486             :     }
+    4487             :   }
+    4488             : 
+    4489         108 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4490           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4491           0 :     prereq_check = false;
+    4492             :   }
+    4493             : 
+    4494         108 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4495           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4496           0 :     prereq_check = false;
+    4497             :   }
+    4498             : 
+    4499         108 :   if (!prereq_check) {
+    4500             : 
+    4501           1 :     res.message = ss.str();
+    4502           1 :     res.success = false;
+    4503             : 
+    4504           1 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4505             : 
+    4506           1 :     return false;
+    4507             : 
+    4508             :   } else {
+    4509             : 
+    4510         107 :     toggleOutput(req.data);
+    4511             : 
+    4512         107 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4513         107 :     res.message = ss.str();
+    4514         107 :     res.success = true;
+    4515             : 
+    4516         107 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4517             : 
+    4518         107 :     publishDiagnostics();
+    4519             : 
+    4520         107 :     return true;
+    4521             :   }
+    4522             : }
+    4523             : 
+    4524             : //}
+    4525             : 
+    4526             : /* callbackArm() //{ */
+    4527             : 
+    4528           7 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4529             : 
+    4530           7 :   if (!is_initialized_) {
+    4531           0 :     return false;
+    4532             :   }
+    4533             : 
+    4534           7 :   ROS_INFO("[ControlManager]: arming by service");
+    4535             : 
+    4536          14 :   std::stringstream ss;
+    4537             : 
+    4538           7 :   if (failsafe_triggered_ || eland_triggered_) {
+    4539             : 
+    4540           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4541             : 
+    4542           0 :     res.message = ss.str();
+    4543           0 :     res.success = false;
+    4544             : 
+    4545           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4546             : 
+    4547           0 :     return true;
+    4548             :   }
+    4549             : 
+    4550           7 :   if (req.data) {
+    4551             : 
+    4552           0 :     ss << "this service is not allowed to arm the UAV";
+    4553           0 :     res.success = false;
+    4554           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4555             : 
+    4556             :   } else {
+    4557             : 
+    4558          14 :     auto [success, message] = arming(false);
+    4559             : 
+    4560           7 :     if (success) {
+    4561             : 
+    4562           7 :       ss << "disarmed";
+    4563           7 :       res.success = true;
+    4564           7 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4565             : 
+    4566             :     } else {
+    4567             : 
+    4568           0 :       ss << "could not disarm: " << message;
+    4569           0 :       res.success = false;
+    4570           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4571             :     }
+    4572             :   }
+    4573             : 
+    4574           7 :   res.message = ss.str();
+    4575             : 
+    4576           7 :   return true;
+    4577             : }
+    4578             : 
+    4579             : //}
+    4580             : 
+    4581             : /* //{ callbackEnableCallbacks() */
+    4582             : 
+    4583          94 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4584             : 
+    4585          94 :   if (!is_initialized_) {
+    4586           0 :     return false;
+    4587             :   }
+    4588             : 
+    4589          94 :   setCallbacks(req.data);
+    4590             : 
+    4591          94 :   std::stringstream ss;
+    4592             : 
+    4593          94 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4594             : 
+    4595          94 :   res.message = ss.str();
+    4596          94 :   res.success = true;
+    4597             : 
+    4598          94 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4599             : 
+    4600          94 :   return true;
+    4601             : }
+    4602             : 
+    4603             : //}
+    4604             : 
+    4605             : /* callbackSetConstraints() //{ */
+    4606             : 
+    4607         110 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4608             : 
+    4609         110 :   if (!is_initialized_) {
+    4610           0 :     res.message = "not initialized";
+    4611           0 :     res.success = false;
+    4612           0 :     return true;
+    4613             :   }
+    4614             : 
+    4615             :   {
+    4616         220 :     std::scoped_lock lock(mutex_constraints_);
+    4617             : 
+    4618         110 :     current_constraints_ = req;
+    4619             : 
+    4620         110 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4621             : 
+    4622         110 :     if (enforced) {
+    4623           0 :       sanitized_constraints_      = enforced.value();
+    4624           0 :       constraints_being_enforced_ = true;
+    4625             :     } else {
+    4626         110 :       sanitized_constraints_      = req;
+    4627         110 :       constraints_being_enforced_ = false;
+    4628             :     }
+    4629             : 
+    4630         110 :     got_constraints_ = true;
+    4631             : 
+    4632         110 :     setConstraintsToControllers(current_constraints_);
+    4633         110 :     setConstraintsToTrackers(sanitized_constraints_);
+    4634             :   }
+    4635             : 
+    4636         110 :   res.message = "setting constraints";
+    4637         110 :   res.success = true;
+    4638             : 
+    4639         110 :   return true;
+    4640             : }
+    4641             : 
+    4642             : //}
+    4643             : 
+    4644             : /* //{ callbackEmergencyReference() */
+    4645             : 
+    4646          93 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4647             : 
+    4648          93 :   if (!is_initialized_) {
+    4649           0 :     return false;
+    4650             :   }
+    4651             : 
+    4652         186 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4653             : 
+    4654          93 :   callbacks_enabled_ = false;
+    4655             : 
+    4656          93 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4657             : 
+    4658         186 :   std::stringstream ss;
+    4659             : 
+    4660             :   // transform the reference to the current frame
+    4661         186 :   mrs_msgs::ReferenceStamped original_reference;
+    4662          93 :   original_reference.header    = req.header;
+    4663          93 :   original_reference.reference = req.reference;
+    4664             : 
+    4665         186 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4666             : 
+    4667          93 :   if (!ret) {
+    4668             : 
+    4669           0 :     ss << "the emergency reference could not be transformed";
+    4670             : 
+    4671           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4672           0 :     res.message = ss.str();
+    4673           0 :     res.success = false;
+    4674           0 :     return true;
+    4675             :   }
+    4676             : 
+    4677          93 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4678             : 
+    4679          93 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4680             : 
+    4681          93 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4682          93 :   req_goto_out.reference = transformed_reference.reference;
+    4683             : 
+    4684             :   {
+    4685         186 :     std::scoped_lock lock(mutex_tracker_list_);
+    4686             : 
+    4687             :     // disable callbacks of all trackers
+    4688          93 :     req_enable_callbacks.data = false;
+    4689         651 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4690         558 :       tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4691             :     }
+    4692             : 
+    4693             :     // enable the callbacks for the active tracker
+    4694          93 :     req_enable_callbacks.data = true;
+    4695          93 :     tracker_list_.at(active_tracker_idx_)
+    4696          93 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4697             : 
+    4698             :     // call the setReference()
+    4699          93 :     tracker_response = tracker_list_.at(active_tracker_idx_)
+    4700          93 :                            ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4701             : 
+    4702             :     // disable the callbacks back again
+    4703          93 :     req_enable_callbacks.data = false;
+    4704          93 :     tracker_list_.at(active_tracker_idx_)
+    4705          93 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4706             : 
+    4707          93 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4708          93 :       res.message = tracker_response->message;
+    4709          93 :       res.success = tracker_response->success;
+    4710             :     } else {
+    4711           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+    4712           0 :       res.message = ss.str();
+    4713           0 :       res.success = false;
+    4714             :     }
+    4715             :   }
+    4716             : 
+    4717          93 :   return true;
+    4718             : }
+    4719             : 
+    4720             : //}
+    4721             : 
+    4722             : /* callbackPirouette() //{ */
+    4723             : 
+    4724           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4725             : 
+    4726           0 :   if (!is_initialized_) {
+    4727           0 :     return false;
+    4728             :   }
+    4729             : 
+    4730             :   // copy member variables
+    4731           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4732             : 
+    4733             :   double uav_heading;
+    4734             :   try {
+    4735           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4736             :   }
+    4737           0 :   catch (...) {
+    4738           0 :     std::stringstream ss;
+    4739           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4740             : 
+    4741           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4742             : 
+    4743           0 :     res.message = ss.str();
+    4744           0 :     res.success = false;
+    4745             : 
+    4746           0 :     return false;
+    4747             :   }
+    4748             : 
+    4749           0 :   if (_pirouette_enabled_) {
+    4750           0 :     res.success = false;
+    4751           0 :     res.message = "already active";
+    4752           0 :     return true;
+    4753             :   }
+    4754             : 
+    4755           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4756             : 
+    4757           0 :     std::stringstream ss;
+    4758           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4759             : 
+    4760           0 :     res.message = ss.str();
+    4761           0 :     res.success = false;
+    4762             : 
+    4763           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4764             : 
+    4765           0 :     return true;
+    4766             :   }
+    4767             : 
+    4768           0 :   _pirouette_enabled_ = true;
+    4769             : 
+    4770           0 :   setCallbacks(false);
+    4771             : 
+    4772           0 :   pirouette_initial_heading_ = uav_heading;
+    4773           0 :   pirouette_iterator_        = 0;
+    4774           0 :   timer_pirouette_.start();
+    4775             : 
+    4776           0 :   res.success = true;
+    4777           0 :   res.message = "activated";
+    4778             : 
+    4779           0 :   return true;
+    4780             : }
+    4781             : 
+    4782             : //}
+    4783             : 
+    4784             : /* callbackUseJoystick() //{ */
+    4785             : 
+    4786           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4787             : 
+    4788           0 :   if (!is_initialized_) {
+    4789           0 :     return false;
+    4790             :   }
+    4791             : 
+    4792           0 :   std::stringstream ss;
+    4793             : 
+    4794             :   {
+    4795           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4796             : 
+    4797           0 :     if (!success) {
+    4798             : 
+    4799           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4800           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4801             : 
+    4802           0 :       res.success = false;
+    4803           0 :       res.message = ss.str();
+    4804             : 
+    4805           0 :       return true;
+    4806             :     }
+    4807             :   }
+    4808             : 
+    4809           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4810             : 
+    4811           0 :   if (!success) {
+    4812             : 
+    4813           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4814           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4815             : 
+    4816           0 :     res.success = false;
+    4817           0 :     res.message = ss.str();
+    4818             : 
+    4819             :     // switch back to hover tracker
+    4820           0 :     switchTracker(_ehover_tracker_name_);
+    4821             : 
+    4822             :     // switch back to safety controller
+    4823           0 :     switchController(_eland_controller_name_);
+    4824             : 
+    4825           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4826             : 
+    4827           0 :     return true;
+    4828             :   }
+    4829             : 
+    4830           0 :   ss << "switched to joystick control";
+    4831             : 
+    4832           0 :   res.success = true;
+    4833           0 :   res.message = ss.str();
+    4834             : 
+    4835           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4836             : 
+    4837           0 :   return true;
+    4838             : }
+    4839             : 
+    4840             : //}
+    4841             : 
+    4842             : /* //{ callbackHover() */
+    4843             : 
+    4844           1 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4845             : 
+    4846           1 :   if (!is_initialized_) {
+    4847           0 :     return false;
+    4848             :   }
+    4849             : 
+    4850           1 :   auto [success, message] = hover();
+    4851             : 
+    4852           1 :   res.success = success;
+    4853           1 :   res.message = message;
+    4854             : 
+    4855           1 :   return true;
+    4856             : }
+    4857             : 
+    4858             : //}
+    4859             : 
+    4860             : /* //{ callbackStartTrajectoryTracking() */
+    4861             : 
+    4862           2 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4863             : 
+    4864           2 :   if (!is_initialized_) {
+    4865           0 :     return false;
+    4866             :   }
+    4867             : 
+    4868           2 :   auto [success, message] = startTrajectoryTracking();
+    4869             : 
+    4870           2 :   res.success = success;
+    4871           2 :   res.message = message;
+    4872             : 
+    4873           2 :   return true;
+    4874             : }
+    4875             : 
+    4876             : //}
+    4877             : 
+    4878             : /* //{ callbackStopTrajectoryTracking() */
+    4879             : 
+    4880           1 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4881             : 
+    4882           1 :   if (!is_initialized_) {
+    4883           0 :     return false;
+    4884             :   }
+    4885             : 
+    4886           1 :   auto [success, message] = stopTrajectoryTracking();
+    4887             : 
+    4888           1 :   res.success = success;
+    4889           1 :   res.message = message;
+    4890             : 
+    4891           1 :   return true;
+    4892             : }
+    4893             : 
+    4894             : //}
+    4895             : 
+    4896             : /* //{ callbackResumeTrajectoryTracking() */
+    4897             : 
+    4898           1 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4899             : 
+    4900           1 :   if (!is_initialized_) {
+    4901           0 :     return false;
+    4902             :   }
+    4903             : 
+    4904           1 :   auto [success, message] = resumeTrajectoryTracking();
+    4905             : 
+    4906           1 :   res.success = success;
+    4907           1 :   res.message = message;
+    4908             : 
+    4909           1 :   return true;
+    4910             : }
+    4911             : 
+    4912             : //}
+    4913             : 
+    4914             : /* //{ callbackGotoTrajectoryStart() */
+    4915             : 
+    4916           2 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4917             : 
+    4918           2 :   if (!is_initialized_) {
+    4919           0 :     return false;
+    4920             :   }
+    4921             : 
+    4922           2 :   auto [success, message] = gotoTrajectoryStart();
+    4923             : 
+    4924           2 :   res.success = success;
+    4925           2 :   res.message = message;
+    4926             : 
+    4927           2 :   return true;
+    4928             : }
+    4929             : 
+    4930             : //}
+    4931             : 
+    4932             : /* //{ callbackTransformReference() */
+    4933             : 
+    4934           1 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4935             : 
+    4936           1 :   if (!is_initialized_) {
+    4937           0 :     return false;
+    4938             :   }
+    4939             : 
+    4940             :   // transform the reference to the current frame
+    4941           2 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4942             : 
+    4943           2 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4944             : 
+    4945           1 :     res.reference = ret.value();
+    4946           1 :     res.message   = "transformation successful";
+    4947           1 :     res.success   = true;
+    4948           1 :     return true;
+    4949             : 
+    4950             :   } else {
+    4951             : 
+    4952           0 :     res.message = "the reference could not be transformed";
+    4953           0 :     res.success = false;
+    4954           0 :     return true;
+    4955             :   }
+    4956             : 
+    4957             :   return true;
+    4958             : }
+    4959             : 
+    4960             : //}
+    4961             : 
+    4962             : /* //{ callbackTransformPose() */
+    4963             : 
+    4964           1 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    4965             : 
+    4966           1 :   if (!is_initialized_) {
+    4967           0 :     return false;
+    4968             :   }
+    4969             : 
+    4970             :   // transform the reference to the current frame
+    4971           2 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    4972             : 
+    4973           2 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    4974             : 
+    4975           1 :     res.pose    = ret.value();
+    4976           1 :     res.message = "transformation successful";
+    4977           1 :     res.success = true;
+    4978           1 :     return true;
+    4979             : 
+    4980             :   } else {
+    4981             : 
+    4982           0 :     res.message = "the pose could not be transformed";
+    4983           0 :     res.success = false;
+    4984           0 :     return true;
+    4985             :   }
+    4986             : 
+    4987             :   return true;
+    4988             : }
+    4989             : 
+    4990             : //}
+    4991             : 
+    4992             : /* //{ callbackTransformVector3() */
+    4993             : 
+    4994           1 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    4995             : 
+    4996           1 :   if (!is_initialized_) {
+    4997           0 :     return false;
+    4998             :   }
+    4999             : 
+    5000             :   // transform the reference to the current frame
+    5001           2 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    5002             : 
+    5003           2 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    5004             : 
+    5005           1 :     res.vector  = ret.value();
+    5006           1 :     res.message = "transformation successful";
+    5007           1 :     res.success = true;
+    5008           1 :     return true;
+    5009             : 
+    5010             :   } else {
+    5011             : 
+    5012           0 :     res.message = "the twist could not be transformed";
+    5013           0 :     res.success = false;
+    5014           0 :     return true;
+    5015             :   }
+    5016             : 
+    5017             :   return true;
+    5018             : }
+    5019             : 
+    5020             : //}
+    5021             : 
+    5022             : /* //{ callbackEnableBumper() */
+    5023             : 
+    5024           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    5025             : 
+    5026           0 :   if (!is_initialized_) {
+    5027           0 :     return false;
+    5028             :   }
+    5029             : 
+    5030           0 :   bumper_enabled_ = req.data;
+    5031             : 
+    5032           0 :   std::stringstream ss;
+    5033             : 
+    5034           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    5035             : 
+    5036           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5037             : 
+    5038           0 :   res.success = true;
+    5039           0 :   res.message = ss.str();
+    5040             : 
+    5041           0 :   return true;
+    5042             : }
+    5043             : 
+    5044             : //}
+    5045             : 
+    5046             : /* //{ callbackUseSafetyArea() */
+    5047             : 
+    5048           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    5049             : 
+    5050           0 :   if (!is_initialized_) {
+    5051           0 :     return false;
+    5052             :   }
+    5053             : 
+    5054           0 :   use_safety_area_ = req.data;
+    5055             : 
+    5056           0 :   std::stringstream ss;
+    5057             : 
+    5058           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    5059             : 
+    5060           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5061             : 
+    5062           0 :   res.success = true;
+    5063           0 :   res.message = ss.str();
+    5064             : 
+    5065           0 :   return true;
+    5066             : }
+    5067             : 
+    5068             : //}
+    5069             : 
+    5070             : /* //{ callbackGetMinZ() */
+    5071             : 
+    5072           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    5073             : 
+    5074           0 :   if (!is_initialized_) {
+    5075           0 :     return false;
+    5076             :   }
+    5077             : 
+    5078           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5079             : 
+    5080           0 :   res.success = true;
+    5081           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    5082             : 
+    5083           0 :   return true;
+    5084             : }
+    5085             : 
+    5086             : //}
+    5087             : 
+    5088             : /* //{ callbackValidateReference() */
+    5089             : 
+    5090           4 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5091             : 
+    5092           4 :   if (!is_initialized_) {
+    5093           0 :     res.message = "not initialized";
+    5094           0 :     res.success = false;
+    5095           0 :     return true;
+    5096             :   }
+    5097             : 
+    5098           4 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5099           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5100           0 :     res.message = "NaNs/infs in input!";
+    5101           0 :     res.success = false;
+    5102           0 :     return true;
+    5103             :   }
+    5104             : 
+    5105             :   // copy member variables
+    5106           8 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5107           8 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5108             : 
+    5109             :   // transform the reference to the current frame
+    5110           8 :   mrs_msgs::ReferenceStamped original_reference;
+    5111           4 :   original_reference.header    = req.reference.header;
+    5112           4 :   original_reference.reference = req.reference.reference;
+    5113             : 
+    5114           8 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5115             : 
+    5116           4 :   if (!ret) {
+    5117             : 
+    5118           1 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5119           1 :     res.message = "the reference could not be transformed";
+    5120           1 :     res.success = false;
+    5121           1 :     return true;
+    5122             :   }
+    5123             : 
+    5124           6 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5125             : 
+    5126           3 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5127           2 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5128           2 :     res.message = "the point is outside of the safety area";
+    5129           2 :     res.success = false;
+    5130           2 :     return true;
+    5131             :   }
+    5132             : 
+    5133           1 :   if (last_tracker_cmd) {
+    5134             : 
+    5135           1 :     mrs_msgs::ReferenceStamped from_point;
+    5136           1 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5137           1 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5138           1 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5139           1 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5140             : 
+    5141           1 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5142           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5143           0 :       res.message = "the path is going outside the safety area";
+    5144           0 :       res.success = false;
+    5145           0 :       return true;
+    5146             :     }
+    5147             :   }
+    5148             : 
+    5149           1 :   res.message = "the reference is ok";
+    5150           1 :   res.success = true;
+    5151           1 :   return true;
+    5152             : }
+    5153             : 
+    5154             : //}
+    5155             : 
+    5156             : /* //{ callbackValidateReference2d() */
+    5157             : 
+    5158        9533 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5159             : 
+    5160        9533 :   if (!is_initialized_) {
+    5161           0 :     res.message = "not initialized";
+    5162           0 :     res.success = false;
+    5163           0 :     return true;
+    5164             :   }
+    5165             : 
+    5166        9533 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5167           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5168           0 :     res.message = "NaNs/infs in input!";
+    5169           0 :     res.success = false;
+    5170           0 :     return true;
+    5171             :   }
+    5172             : 
+    5173             :   // copy member variables
+    5174       19066 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5175       19066 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5176             : 
+    5177             :   // transform the reference to the current frame
+    5178       19066 :   mrs_msgs::ReferenceStamped original_reference;
+    5179        9533 :   original_reference.header    = req.reference.header;
+    5180        9533 :   original_reference.reference = req.reference.reference;
+    5181             : 
+    5182       19066 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5183             : 
+    5184        9533 :   if (!ret) {
+    5185             : 
+    5186           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5187           0 :     res.message = "the reference could not be transformed";
+    5188           0 :     res.success = false;
+    5189           0 :     return true;
+    5190             :   }
+    5191             : 
+    5192       19066 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5193             : 
+    5194        9533 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5195          74 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5196          74 :     res.message = "the point is outside of the safety area";
+    5197          74 :     res.success = false;
+    5198          74 :     return true;
+    5199             :   }
+    5200             : 
+    5201        9459 :   if (last_tracker_cmd) {
+    5202             : 
+    5203          31 :     mrs_msgs::ReferenceStamped from_point;
+    5204          31 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5205          31 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5206          31 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5207          31 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5208             : 
+    5209          31 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5210           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5211           0 :       res.message = "the path is going outside the safety area";
+    5212           0 :       res.success = false;
+    5213           0 :       return true;
+    5214             :     }
+    5215             :   }
+    5216             : 
+    5217        9459 :   res.message = "the reference is ok";
+    5218        9459 :   res.success = true;
+    5219        9459 :   return true;
+    5220             : }
+    5221             : 
+    5222             : //}
+    5223             : 
+    5224             : /* //{ callbackValidateReferenceList() */
+    5225             : 
+    5226           2 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+    5227             : 
+    5228           2 :   if (!is_initialized_) {
+    5229           0 :     res.message = "not initialized";
+    5230           0 :     return false;
+    5231             :   }
+    5232             : 
+    5233             :   // copy member variables
+    5234           4 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5235           4 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5236             : 
+    5237             :   // get the transformer
+    5238           4 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+    5239             : 
+    5240           2 :   if (!ret) {
+    5241             : 
+    5242           1 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5243           1 :     res.message = "could not find transform";
+    5244           1 :     return false;
+    5245             :   }
+    5246             : 
+    5247           1 :   geometry_msgs::TransformStamped tf = ret.value();
+    5248             : 
+    5249           5 :   for (int i = 0; i < int(req.list.list.size()); i++) {
+    5250             : 
+    5251           4 :     res.success.push_back(true);
+    5252             : 
+    5253           8 :     mrs_msgs::ReferenceStamped original_reference;
+    5254           4 :     original_reference.header    = req.list.header;
+    5255           4 :     original_reference.reference = req.list.list.at(i);
+    5256             : 
+    5257           4 :     res.success.at(i) = validateReference(original_reference.reference, "ControlManager", "reference_list");
+    5258             : 
+    5259           8 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5260             : 
+    5261           4 :     if (!ret) {
+    5262             : 
+    5263           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5264           0 :       res.success.at(i) = false;
+    5265             :     }
+    5266             : 
+    5267           8 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5268             : 
+    5269           4 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5270           2 :       res.success.at(i) = false;
+    5271             :     }
+    5272             : 
+    5273           4 :     if (last_tracker_cmd) {
+    5274             : 
+    5275           8 :       mrs_msgs::ReferenceStamped from_point;
+    5276           4 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5277           4 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5278           4 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5279           4 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5280             : 
+    5281           4 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5282           2 :         res.success.at(i) = false;
+    5283             :       }
+    5284             :     }
+    5285             :   }
+    5286             : 
+    5287           1 :   res.message = "references were checked";
+    5288           1 :   return true;
+    5289             : }
+    5290             : 
+    5291             : //}
+    5292             : 
+    5293             : // | -------------- setpoint topics and services -------------- |
+    5294             : 
+    5295             : /* //{ callbackReferenceService() */
+    5296             : 
+    5297           2 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5298             : 
+    5299           2 :   if (!is_initialized_) {
+    5300           0 :     res.message = "not initialized";
+    5301           0 :     res.success = false;
+    5302           0 :     return true;
+    5303             :   }
+    5304             : 
+    5305           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5306           6 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5307             : 
+    5308           4 :   mrs_msgs::ReferenceStamped des_reference;
+    5309           2 :   des_reference.header    = req.header;
+    5310           2 :   des_reference.reference = req.reference;
+    5311             : 
+    5312           4 :   auto [success, message] = setReference(des_reference);
+    5313             : 
+    5314           2 :   res.success = success;
+    5315           2 :   res.message = message;
+    5316             : 
+    5317           2 :   return true;
+    5318             : }
+    5319             : 
+    5320             : //}
+    5321             : 
+    5322             : /* //{ callbackReferenceTopic() */
+    5323             : 
+    5324           1 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5325             : 
+    5326           1 :   if (!is_initialized_) {
+    5327           0 :     return;
+    5328             :   }
+    5329             : 
+    5330           3 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5331           2 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5332             : 
+    5333           1 :   setReference(*msg);
+    5334             : }
+    5335             : 
+    5336             : //}
+    5337             : 
+    5338             : /* //{ callbackVelocityReferenceService() */
+    5339             : 
+    5340         643 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5341             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5342             : 
+    5343         643 :   if (!is_initialized_) {
+    5344           0 :     res.message = "not initialized";
+    5345           0 :     res.success = false;
+    5346           0 :     return true;
+    5347             :   }
+    5348             : 
+    5349        1929 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5350        1929 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5351             : 
+    5352        1286 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5353         643 :   des_reference = req.reference;
+    5354             : 
+    5355         643 :   auto [success, message] = setVelocityReference(des_reference);
+    5356             : 
+    5357         643 :   res.success = success;
+    5358         643 :   res.message = message;
+    5359             : 
+    5360         643 :   return true;
+    5361             : }
+    5362             : 
+    5363             : //}
+    5364             : 
+    5365             : /* //{ callbackVelocityReferenceTopic() */
+    5366             : 
+    5367          94 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5368             : 
+    5369          94 :   if (!is_initialized_) {
+    5370           0 :     return;
+    5371             :   }
+    5372             : 
+    5373         282 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5374         188 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5375             : 
+    5376          94 :   setVelocityReference(*msg);
+    5377             : }
+    5378             : 
+    5379             : //}
+    5380             : 
+    5381             : /* //{ callbackTrajectoryReferenceService() */
+    5382             : 
+    5383           4 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5384             : 
+    5385           4 :   if (!is_initialized_) {
+    5386           0 :     res.message = "not initialized";
+    5387           0 :     res.success = false;
+    5388           0 :     return true;
+    5389             :   }
+    5390             : 
+    5391          12 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5392          12 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5393             : 
+    5394           8 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5395             : 
+    5396           4 :   res.success          = success;
+    5397           4 :   res.message          = message;
+    5398           4 :   res.modified         = modified;
+    5399           4 :   res.tracker_names    = tracker_names;
+    5400           4 :   res.tracker_messages = tracker_messages;
+    5401             : 
+    5402          28 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5403          24 :     res.tracker_successes.push_back(tracker_successes.at(i));
+    5404             :   }
+    5405             : 
+    5406           4 :   return true;
+    5407             : }
+    5408             : 
+    5409             : //}
+    5410             : 
+    5411             : /* //{ callbackTrajectoryReferenceTopic() */
+    5412             : 
+    5413           2 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5414             : 
+    5415           2 :   if (!is_initialized_) {
+    5416           0 :     return;
+    5417             :   }
+    5418             : 
+    5419           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5420           4 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5421             : 
+    5422           2 :   setTrajectoryReference(*msg);
+    5423             : }
+    5424             : 
+    5425             : //}
+    5426             : 
+    5427             : // | ------------- human-callable "goto" services ------------- |
+    5428             : 
+    5429             : /* //{ callbackGoto() */
+    5430             : 
+    5431          27 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5432             : 
+    5433          27 :   if (!is_initialized_) {
+    5434           0 :     res.message = "not initialized";
+    5435           0 :     res.success = false;
+    5436           0 :     return true;
+    5437             :   }
+    5438             : 
+    5439          81 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5440          81 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5441             : 
+    5442          54 :   mrs_msgs::ReferenceStamped des_reference;
+    5443          27 :   des_reference.header.frame_id      = "";
+    5444          27 :   des_reference.header.stamp         = ros::Time(0);
+    5445          27 :   des_reference.reference.position.x = req.goal.at(REF_X);
+    5446          27 :   des_reference.reference.position.y = req.goal.at(REF_Y);
+    5447          27 :   des_reference.reference.position.z = req.goal.at(REF_Z);
+    5448          27 :   des_reference.reference.heading    = req.goal.at(REF_HEADING);
+    5449             : 
+    5450          54 :   auto [success, message] = setReference(des_reference);
+    5451             : 
+    5452          27 :   res.success = success;
+    5453          27 :   res.message = message;
+    5454             : 
+    5455          27 :   return true;
+    5456             : }
+    5457             : 
+    5458             : //}
+    5459             : 
+    5460             : /* //{ callbackGotoFcu() */
+    5461             : 
+    5462           0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5463             : 
+    5464           0 :   if (!is_initialized_) {
+    5465           0 :     res.message = "not initialized";
+    5466           0 :     res.success = false;
+    5467           0 :     return true;
+    5468             :   }
+    5469             : 
+    5470           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5471           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5472             : 
+    5473           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5474           0 :   des_reference.header.frame_id      = "fcu_untilted";
+    5475           0 :   des_reference.header.stamp         = ros::Time(0);
+    5476           0 :   des_reference.reference.position.x = req.goal.at(REF_X);
+    5477           0 :   des_reference.reference.position.y = req.goal.at(REF_Y);
+    5478           0 :   des_reference.reference.position.z = req.goal.at(REF_Z);
+    5479           0 :   des_reference.reference.heading    = req.goal.at(REF_HEADING);
+    5480             : 
+    5481           0 :   auto [success, message] = setReference(des_reference);
+    5482             : 
+    5483           0 :   res.success = success;
+    5484           0 :   res.message = message;
+    5485             : 
+    5486           0 :   return true;
+    5487             : }
+    5488             : 
+    5489             : //}
+    5490             : 
+    5491             : /* //{ callbackGotoRelative() */
+    5492             : 
+    5493          25 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5494             : 
+    5495          25 :   if (!is_initialized_) {
+    5496           0 :     res.message = "not initialized";
+    5497           0 :     res.success = false;
+    5498           0 :     return true;
+    5499             :   }
+    5500             : 
+    5501          75 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5502          75 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5503             : 
+    5504          50 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5505             : 
+    5506          25 :   if (!last_tracker_cmd) {
+    5507           0 :     res.message = "not flying";
+    5508           0 :     res.success = false;
+    5509           0 :     return true;
+    5510             :   }
+    5511             : 
+    5512          50 :   mrs_msgs::ReferenceStamped des_reference;
+    5513          25 :   des_reference.header.frame_id      = "";
+    5514          25 :   des_reference.header.stamp         = ros::Time(0);
+    5515          25 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal.at(REF_X);
+    5516          25 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal.at(REF_Y);
+    5517          25 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal.at(REF_Z);
+    5518          25 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal.at(REF_HEADING);
+    5519             : 
+    5520          50 :   auto [success, message] = setReference(des_reference);
+    5521             : 
+    5522          25 :   res.success = success;
+    5523          25 :   res.message = message;
+    5524             : 
+    5525          25 :   return true;
+    5526             : }
+    5527             : 
+    5528             : //}
+    5529             : 
+    5530             : /* //{ callbackGotoAltitude() */
+    5531             : 
+    5532           2 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5533             : 
+    5534           2 :   if (!is_initialized_) {
+    5535           0 :     res.message = "not initialized";
+    5536           0 :     res.success = false;
+    5537           0 :     return true;
+    5538             :   }
+    5539             : 
+    5540           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5541           6 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5542             : 
+    5543           4 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5544             : 
+    5545           2 :   if (!last_tracker_cmd) {
+    5546           0 :     res.message = "not flying";
+    5547           0 :     res.success = false;
+    5548           0 :     return true;
+    5549             :   }
+    5550             : 
+    5551           4 :   mrs_msgs::ReferenceStamped des_reference;
+    5552           2 :   des_reference.header.frame_id      = "";
+    5553           2 :   des_reference.header.stamp         = ros::Time(0);
+    5554           2 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5555           2 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5556           2 :   des_reference.reference.position.z = req.goal;
+    5557           2 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5558             : 
+    5559           4 :   auto [success, message] = setReference(des_reference);
+    5560             : 
+    5561           2 :   res.success = success;
+    5562           2 :   res.message = message;
+    5563             : 
+    5564           2 :   return true;
+    5565             : }
+    5566             : 
+    5567             : //}
+    5568             : 
+    5569             : /* //{ callbackSetHeading() */
+    5570             : 
+    5571           4 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5572             : 
+    5573           4 :   if (!is_initialized_) {
+    5574           0 :     res.message = "not initialized";
+    5575           0 :     res.success = false;
+    5576           0 :     return true;
+    5577             :   }
+    5578             : 
+    5579          12 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5580          12 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5581             : 
+    5582           8 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5583             : 
+    5584           4 :   if (!last_tracker_cmd) {
+    5585           0 :     res.message = "not flying";
+    5586           0 :     res.success = false;
+    5587           0 :     return true;
+    5588             :   }
+    5589             : 
+    5590           8 :   mrs_msgs::ReferenceStamped des_reference;
+    5591           4 :   des_reference.header.frame_id      = "";
+    5592           4 :   des_reference.header.stamp         = ros::Time(0);
+    5593           4 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5594           4 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5595           4 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5596           4 :   des_reference.reference.heading    = req.goal;
+    5597             : 
+    5598           8 :   auto [success, message] = setReference(des_reference);
+    5599             : 
+    5600           4 :   res.success = success;
+    5601           4 :   res.message = message;
+    5602             : 
+    5603           4 :   return true;
+    5604             : }
+    5605             : 
+    5606             : //}
+    5607             : 
+    5608             : /* //{ callbackSetHeadingRelative() */
+    5609             : 
+    5610           3 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5611             : 
+    5612           3 :   if (!is_initialized_) {
+    5613           0 :     res.message = "not initialized";
+    5614           0 :     res.success = false;
+    5615           0 :     return true;
+    5616             :   }
+    5617             : 
+    5618           9 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5619           9 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5620             : 
+    5621           6 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5622             : 
+    5623           3 :   if (!last_tracker_cmd) {
+    5624           0 :     res.message = "not flying";
+    5625           0 :     res.success = false;
+    5626           0 :     return true;
+    5627             :   }
+    5628             : 
+    5629           6 :   mrs_msgs::ReferenceStamped des_reference;
+    5630           3 :   des_reference.header.frame_id      = "";
+    5631           3 :   des_reference.header.stamp         = ros::Time(0);
+    5632           3 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5633           3 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5634           3 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5635           3 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5636             : 
+    5637           6 :   auto [success, message] = setReference(des_reference);
+    5638             : 
+    5639           3 :   res.success = success;
+    5640           3 :   res.message = message;
+    5641             : 
+    5642           3 :   return true;
+    5643             : }
+    5644             : 
+    5645             : //}
+    5646             : 
+    5647             : // --------------------------------------------------------------
+    5648             : // |                          routines                          |
+    5649             : // --------------------------------------------------------------
+    5650             : 
+    5651             : /* setReference() //{ */
+    5652             : 
+    5653          64 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5654             : 
+    5655         128 :   std::stringstream ss;
+    5656             : 
+    5657          64 :   if (!callbacks_enabled_) {
+    5658           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5659           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5660           0 :     return std::tuple(false, ss.str());
+    5661             :   }
+    5662             : 
+    5663          64 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5664           0 :     ss << "incoming reference is not finite!!!";
+    5665           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5666           0 :     return std::tuple(false, ss.str());
+    5667             :   }
+    5668             : 
+    5669             :   // copy member variables
+    5670         128 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5671         128 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5672             : 
+    5673             :   // transform the reference to the current frame
+    5674         128 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5675             : 
+    5676          64 :   if (!ret) {
+    5677             : 
+    5678           0 :     ss << "the 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             : 
+    5683         128 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5684             : 
+    5685             :   // safety area check
+    5686          64 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5687           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5688           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5689           0 :     return std::tuple(false, ss.str());
+    5690             :   }
+    5691             : 
+    5692          64 :   if (last_tracker_cmd) {
+    5693             : 
+    5694          64 :     mrs_msgs::ReferenceStamped from_point;
+    5695          64 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5696          64 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5697          64 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5698          64 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5699             : 
+    5700          64 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5701           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5702           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5703           0 :       return std::tuple(false, ss.str());
+    5704             :     }
+    5705             :   }
+    5706             : 
+    5707          64 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5708             : 
+    5709             :   // prepare the message for current tracker
+    5710          64 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5711          64 :   reference_request.reference = transformed_reference.reference;
+    5712             : 
+    5713             :   {
+    5714         128 :     std::scoped_lock lock(mutex_tracker_list_);
+    5715             : 
+    5716          64 :     ROS_INFO("[ControlManager]: setting reference to x=%.2f, y=%.2f, z=%.2f, hdg=%.2f (expressed in '%s')", reference_request.reference.position.x,
+    5717             :              reference_request.reference.position.y, reference_request.reference.position.z, reference_request.reference.heading,
+    5718             :              transformed_reference.header.frame_id.c_str());
+    5719             : 
+    5720          64 :     tracker_response = tracker_list_.at(active_tracker_idx_)
+    5721          64 :                            ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5722             : 
+    5723          64 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5724             : 
+    5725         128 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5726             : 
+    5727             :     } else {
+    5728             : 
+    5729           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+    5730           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5731           0 :       return std::tuple(false, ss.str());
+    5732             :     }
+    5733             :   }
+    5734             : }
+    5735             : 
+    5736             : //}
+    5737             : 
+    5738             : /* setVelocityReference() //{ */
+    5739             : 
+    5740         737 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5741             : 
+    5742        1474 :   std::stringstream ss;
+    5743             : 
+    5744         737 :   if (!callbacks_enabled_) {
+    5745           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5746           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5747           0 :     return std::tuple(false, ss.str());
+    5748             :   }
+    5749             : 
+    5750         737 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5751           0 :     ss << "velocity command is not valid!";
+    5752           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5753           0 :     return std::tuple(false, ss.str());
+    5754             :   }
+    5755             : 
+    5756             :   {
+    5757         737 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5758             : 
+    5759         737 :     if (!last_tracker_cmd_) {
+    5760           0 :       ss << "could not set velocity command, not flying!";
+    5761           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5762           0 :       return std::tuple(false, ss.str());
+    5763             :     }
+    5764             :   }
+    5765             : 
+    5766             :   // copy member variables
+    5767        1474 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5768        1474 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5769             : 
+    5770             :   // | -- transform the velocity reference to the current frame - |
+    5771             : 
+    5772        1474 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5773             : 
+    5774        1474 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5775             : 
+    5776        1474 :   geometry_msgs::TransformStamped tf;
+    5777             : 
+    5778         737 :   if (!ret) {
+    5779           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5780           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5781           0 :     return std::tuple(false, ss.str());
+    5782             :   } else {
+    5783         737 :     tf = ret.value();
+    5784             :   }
+    5785             : 
+    5786             :   // transform the velocity
+    5787             :   {
+    5788         737 :     geometry_msgs::Vector3Stamped velocity;
+    5789         737 :     velocity.header   = reference_in.header;
+    5790         737 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5791         737 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5792         737 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5793             : 
+    5794         737 :     auto ret = transformer_->transform(velocity, tf);
+    5795             : 
+    5796         737 :     if (!ret) {
+    5797             : 
+    5798           0 :       ss << "the velocity reference could not be transformed";
+    5799           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5800           0 :       return std::tuple(false, ss.str());
+    5801             : 
+    5802             :     } else {
+    5803         737 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5804         737 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5805         737 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5806             :     }
+    5807             :   }
+    5808             : 
+    5809             :   // transform the z and the heading
+    5810             :   {
+    5811         737 :     geometry_msgs::PoseStamped pose;
+    5812         737 :     pose.header           = reference_in.header;
+    5813         737 :     pose.pose.position.x  = 0;
+    5814         737 :     pose.pose.position.y  = 0;
+    5815         737 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5816         737 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5817             : 
+    5818         737 :     auto ret = transformer_->transform(pose, tf);
+    5819             : 
+    5820         737 :     if (!ret) {
+    5821             : 
+    5822           0 :       ss << "the velocity reference could not be transformed";
+    5823           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5824           0 :       return std::tuple(false, ss.str());
+    5825             : 
+    5826             :     } else {
+    5827         737 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5828         737 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5829             :     }
+    5830             :   }
+    5831             : 
+    5832             :   // the heading rate doees not need to be transformed
+    5833         737 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5834             : 
+    5835         737 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5836         737 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5837             : 
+    5838        1474 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5839             : 
+    5840         737 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5841             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5842             : 
+    5843             :   // safety area check
+    5844         737 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5845           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5846           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5847           0 :     return std::tuple(false, ss.str());
+    5848             :   }
+    5849             : 
+    5850         737 :   if (last_tracker_cmd) {
+    5851             : 
+    5852         737 :     mrs_msgs::ReferenceStamped from_point;
+    5853         737 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5854         737 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5855         737 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5856         737 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5857             : 
+    5858         737 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5859           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5860           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5861           0 :       return std::tuple(false, ss.str());
+    5862             :     }
+    5863             :   }
+    5864             : 
+    5865         737 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5866             : 
+    5867             :   // prepare the message for current tracker
+    5868         737 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5869         737 :   reference_request.reference = transformed_reference.reference;
+    5870             : 
+    5871             :   {
+    5872        1474 :     std::scoped_lock lock(mutex_tracker_list_);
+    5873             : 
+    5874             :     tracker_response =
+    5875         737 :         tracker_list_.at(active_tracker_idx_)
+    5876         737 :             ->setVelocityReference(mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5877             : 
+    5878         737 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5879             : 
+    5880        1474 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5881             : 
+    5882             :     } else {
+    5883             : 
+    5884           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setVelocityReference()' function!";
+    5885           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5886           0 :       return std::tuple(false, ss.str());
+    5887             :     }
+    5888             :   }
+    5889             : }
+    5890             : 
+    5891             : //}
+    5892             : 
+    5893             : /* setTrajectoryReference() //{ */
+    5894             : 
+    5895           6 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5896             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5897             : 
+    5898          12 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5899             : 
+    5900          12 :   std::stringstream ss;
+    5901             : 
+    5902           6 :   if (!callbacks_enabled_) {
+    5903           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5904           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5905           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5906             :   }
+    5907             : 
+    5908             :   /* validate the size and check for NaNs //{ */
+    5909             : 
+    5910             :   // check for the size 0, which is invalid
+    5911           6 :   if (trajectory_in.points.size() == 0) {
+    5912             : 
+    5913           0 :     ss << "can not load trajectory with size 0";
+    5914           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5915           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5916             :   }
+    5917             : 
+    5918         643 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5919             : 
+    5920             :     // check the point for NaN/inf
+    5921         637 :     bool valid = validateReference(trajectory_in.points.at(i), "ControlManager", "trajectory_in.points");
+    5922             : 
+    5923         637 :     if (!valid) {
+    5924             : 
+    5925           0 :       ss << "trajectory contains NaNs/infs.";
+    5926           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5927           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5928             :     }
+    5929             :   }
+    5930             : 
+    5931             :   //}
+    5932             : 
+    5933             :   /* publish the debugging topics of the original trajectory //{ */
+    5934             : 
+    5935             :   {
+    5936             : 
+    5937          12 :     geometry_msgs::PoseArray debug_trajectory_out;
+    5938           6 :     debug_trajectory_out.header = trajectory_in.header;
+    5939             : 
+    5940           6 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    5941             : 
+    5942           6 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    5943           4 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    5944             :     }
+    5945             : 
+    5946         637 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5947             : 
+    5948         631 :       geometry_msgs::Pose new_pose;
+    5949             : 
+    5950         631 :       new_pose.position.x = trajectory_in.points.at(i).position.x;
+    5951         631 :       new_pose.position.y = trajectory_in.points.at(i).position.y;
+    5952         631 :       new_pose.position.z = trajectory_in.points.at(i).position.z;
+    5953             : 
+    5954         631 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points.at(i).heading);
+    5955             : 
+    5956         631 :       debug_trajectory_out.poses.push_back(new_pose);
+    5957             :     }
+    5958             : 
+    5959           6 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    5960             : 
+    5961          12 :     visualization_msgs::MarkerArray msg_out;
+    5962             : 
+    5963          12 :     visualization_msgs::Marker marker;
+    5964             : 
+    5965           6 :     marker.header = trajectory_in.header;
+    5966             : 
+    5967           6 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    5968             : 
+    5969           6 :     if (marker.header.frame_id == "") {
+    5970           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    5971             :     }
+    5972             : 
+    5973           6 :     if (marker.header.stamp == ros::Time(0)) {
+    5974           4 :       marker.header.stamp = ros::Time::now();
+    5975             :     }
+    5976             : 
+    5977           6 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    5978           6 :     marker.color.a          = 1;
+    5979           6 :     marker.scale.x          = 0.05;
+    5980           6 :     marker.color.r          = 0;
+    5981           6 :     marker.color.g          = 1;
+    5982           6 :     marker.color.b          = 0;
+    5983           6 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    5984             : 
+    5985         637 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5986             : 
+    5987         631 :       geometry_msgs::Point point1;
+    5988             : 
+    5989         631 :       point1.x = trajectory_in.points.at(i).position.x;
+    5990         631 :       point1.y = trajectory_in.points.at(i).position.y;
+    5991         631 :       point1.z = trajectory_in.points.at(i).position.z;
+    5992             : 
+    5993         631 :       marker.points.push_back(point1);
+    5994             : 
+    5995         631 :       geometry_msgs::Point point2;
+    5996             : 
+    5997         631 :       point2.x = trajectory_in.points.at(i + 1).position.x;
+    5998         631 :       point2.y = trajectory_in.points.at(i + 1).position.y;
+    5999         631 :       point2.z = trajectory_in.points.at(i + 1).position.z;
+    6000             : 
+    6001         631 :       marker.points.push_back(point2);
+    6002             :     }
+    6003             : 
+    6004           6 :     msg_out.markers.push_back(marker);
+    6005             : 
+    6006           6 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    6007             :   }
+    6008             : 
+    6009             :   //}
+    6010             : 
+    6011          12 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    6012             : 
+    6013           6 :   int trajectory_size = int(processed_trajectory.points.size());
+    6014             : 
+    6015           6 :   bool trajectory_modified = false;
+    6016             : 
+    6017             :   /* safety area check //{ */
+    6018             : 
+    6019           6 :   if (use_safety_area_) {
+    6020             : 
+    6021           5 :     int last_valid_idx    = 0;
+    6022           5 :     int first_invalid_idx = -1;
+    6023             : 
+    6024           5 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    6025           5 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    6026             : 
+    6027         621 :     for (int i = 0; i < trajectory_size; i++) {
+    6028             : 
+    6029         616 :       if (_snap_trajectory_to_safety_area_) {
+    6030             : 
+    6031             :         // saturate the trajectory to min and max Z
+    6032           0 :         if (processed_trajectory.points.at(i).position.z < min_z) {
+    6033             : 
+    6034           0 :           processed_trajectory.points.at(i).position.z = min_z;
+    6035           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    6036           0 :           trajectory_modified = true;
+    6037             :         }
+    6038             : 
+    6039           0 :         if (processed_trajectory.points.at(i).position.z > max_z) {
+    6040             : 
+    6041           0 :           processed_trajectory.points.at(i).position.z = max_z;
+    6042           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    6043           0 :           trajectory_modified = true;
+    6044             :         }
+    6045             :       }
+    6046             : 
+    6047             :       // check the point against the safety area
+    6048         616 :       mrs_msgs::ReferenceStamped des_reference;
+    6049         616 :       des_reference.header    = processed_trajectory.header;
+    6050         616 :       des_reference.reference = processed_trajectory.points.at(i);
+    6051             : 
+    6052         616 :       if (!isPointInSafetyArea3d(des_reference)) {
+    6053             : 
+    6054           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    6055           0 :         trajectory_modified = true;
+    6056             : 
+    6057             :         // the first invalid point
+    6058           0 :         if (first_invalid_idx == -1) {
+    6059             : 
+    6060           0 :           first_invalid_idx = i;
+    6061             : 
+    6062           0 :           last_valid_idx = i - 1;
+    6063             :         }
+    6064             : 
+    6065             :         // the point is ok
+    6066             :       } else {
+    6067             : 
+    6068             :         // we found a point, which is ok, after finding a point which was not ok
+    6069         616 :         if (first_invalid_idx != -1) {
+    6070             : 
+    6071             :           // special case, we had no valid point so far
+    6072           0 :           if (last_valid_idx == -1) {
+    6073             : 
+    6074           0 :             ss << "the trajectory starts outside of the safety area!";
+    6075           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6076           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6077             : 
+    6078             :             // we have a valid point in the past
+    6079             :           } else {
+    6080             : 
+    6081           0 :             if (!_snap_trajectory_to_safety_area_) {
+    6082           0 :               break;
+    6083             :             }
+    6084             : 
+    6085           0 :             bool interpolation_success = true;
+    6086             : 
+    6087             :             // iterpolate between the last valid point and this new valid point
+    6088           0 :             double angle = atan2((processed_trajectory.points.at(i).position.y - processed_trajectory.points.at(last_valid_idx).position.y),
+    6089           0 :                                  (processed_trajectory.points.at(i).position.x - processed_trajectory.points.at(last_valid_idx).position.x));
+    6090             : 
+    6091           0 :             double dist_two_points = mrs_lib::geometry::dist(
+    6092           0 :                 vec2_t(processed_trajectory.points.at(i).position.x, processed_trajectory.points.at(i).position.y),
+    6093           0 :                 vec2_t(processed_trajectory.points.at(last_valid_idx).position.x, processed_trajectory.points.at(last_valid_idx).position.y));
+    6094           0 :             double step = dist_two_points / (i - last_valid_idx);
+    6095             : 
+    6096           0 :             for (int j = last_valid_idx; j < i; j++) {
+    6097             : 
+    6098           0 :               mrs_msgs::ReferenceStamped temp_point;
+    6099           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    6100           0 :               temp_point.reference.position.x = processed_trajectory.points.at(last_valid_idx).position.x + (j - last_valid_idx) * cos(angle) * step;
+    6101           0 :               temp_point.reference.position.y = processed_trajectory.points.at(last_valid_idx).position.y + (j - last_valid_idx) * sin(angle) * step;
+    6102             : 
+    6103           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    6104             : 
+    6105           0 :                 interpolation_success = false;
+    6106           0 :                 break;
+    6107             : 
+    6108             :               } else {
+    6109             : 
+    6110           0 :                 processed_trajectory.points.at(j).position.x = temp_point.reference.position.x;
+    6111           0 :                 processed_trajectory.points.at(j).position.y = temp_point.reference.position.y;
+    6112             :               }
+    6113             :             }
+    6114             : 
+    6115           0 :             if (!interpolation_success) {
+    6116           0 :               break;
+    6117             :             }
+    6118             :           }
+    6119             : 
+    6120           0 :           first_invalid_idx = -1;
+    6121             :         }
+    6122             :       }
+    6123             :     }
+    6124             : 
+    6125             :     // special case, the trajectory does not end with a valid point
+    6126           5 :     if (first_invalid_idx != -1) {
+    6127             : 
+    6128             :       // super special case, the whole trajectory is invalid
+    6129           0 :       if (first_invalid_idx == 0) {
+    6130             : 
+    6131           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6132           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6133           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6134             : 
+    6135             :         // there is a good portion of the trajectory in the beginning
+    6136             :       } else {
+    6137             : 
+    6138           0 :         trajectory_size = last_valid_idx + 1;
+    6139           0 :         processed_trajectory.points.resize(trajectory_size);
+    6140           0 :         trajectory_modified = true;
+    6141             :       }
+    6142             :     }
+    6143             :   }
+    6144             : 
+    6145           6 :   if (trajectory_size == 0) {
+    6146             : 
+    6147           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6148           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6149           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6150             :   }
+    6151             : 
+    6152             :   //}
+    6153             : 
+    6154             :   /* transform the trajectory to the current control frame //{ */
+    6155             : 
+    6156           6 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6157             : 
+    6158           6 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6159           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6160             :   } else {
+    6161           6 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6162             :   }
+    6163             : 
+    6164           6 :   if (!tf_traj_state) {
+    6165           0 :     ss << "could not create TF transformer for the trajectory";
+    6166           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6167           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6168             :   }
+    6169             : 
+    6170           6 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6171             : 
+    6172         643 :   for (int i = 0; i < trajectory_size; i++) {
+    6173             : 
+    6174         637 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6175         637 :     trajectory_point.header    = processed_trajectory.header;
+    6176         637 :     trajectory_point.reference = processed_trajectory.points.at(i);
+    6177             : 
+    6178         637 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6179             : 
+    6180         637 :     if (!ret) {
+    6181             : 
+    6182           0 :       ss << "trajectory cannnot be transformed";
+    6183           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6184           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6185             : 
+    6186             :     } else {
+    6187             : 
+    6188             :       // transform the points in the trajectory to the current frame
+    6189         637 :       processed_trajectory.points.at(i) = ret.value().reference;
+    6190             :     }
+    6191             :   }
+    6192             : 
+    6193             :   //}
+    6194             : 
+    6195           6 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6196          12 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6197             : 
+    6198             :   // check for empty trajectory
+    6199           6 :   if (processed_trajectory.points.size() == 0) {
+    6200           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6201           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6202           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6203             :   }
+    6204             : 
+    6205             :   // prepare the message for current tracker
+    6206           6 :   request.trajectory = processed_trajectory;
+    6207             : 
+    6208             :   bool                     success;
+    6209          12 :   std::string              message;
+    6210             :   bool                     modified;
+    6211          12 :   std::vector<std::string> tracker_names;
+    6212          12 :   std::vector<bool>        tracker_successes;
+    6213          12 :   std::vector<std::string> tracker_messages;
+    6214             : 
+    6215             :   {
+    6216          12 :     std::scoped_lock lock(mutex_tracker_list_);
+    6217             : 
+    6218             :     // set the trajectory to the currently active tracker
+    6219             :     response =
+    6220           6 :         tracker_list_.at(active_tracker_idx_)
+    6221           6 :             ->setTrajectoryReference(mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6222             : 
+    6223           6 :     tracker_names.push_back(_tracker_names_.at(active_tracker_idx_));
+    6224             : 
+    6225           6 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6226             : 
+    6227           5 :       success  = response->success;
+    6228           5 :       message  = response->message;
+    6229           5 :       modified = response->modified || trajectory_modified;
+    6230           5 :       tracker_successes.push_back(response->success);
+    6231           5 :       tracker_messages.push_back(response->message);
+    6232             : 
+    6233             :     } else {
+    6234             : 
+    6235           1 :       ss << "the active tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setTrajectoryReference()' function!";
+    6236           1 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6237             : 
+    6238           1 :       success  = true;
+    6239           1 :       message  = ss.str();
+    6240           1 :       modified = false;
+    6241           1 :       tracker_successes.push_back(false);
+    6242           1 :       tracker_messages.push_back(ss.str());
+    6243             :     }
+    6244             : 
+    6245             :     // set the trajectory to the non-active trackers
+    6246          42 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6247             : 
+    6248          36 :       if (i != active_tracker_idx_) {
+    6249             : 
+    6250          30 :         tracker_names.push_back(_tracker_names_.at(i));
+    6251             : 
+    6252          90 :         response = tracker_list_.at(i)->setTrajectoryReference(
+    6253          90 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6254             : 
+    6255          30 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6256             : 
+    6257           1 :           tracker_successes.push_back(response->success);
+    6258           1 :           tracker_messages.push_back(response->message);
+    6259             : 
+    6260           1 :           if (response->success) {
+    6261           2 :             std::stringstream ss;
+    6262           1 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_.at(i);
+    6263           1 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6264             :           }
+    6265             : 
+    6266             :         } else {
+    6267             : 
+    6268          29 :           std::stringstream ss;
+    6269          29 :           ss << "the tracker \"" << _tracker_names_.at(i) << "\" does not implement setTrajectoryReference()";
+    6270          29 :           tracker_successes.push_back(false);
+    6271          29 :           tracker_messages.push_back(ss.str());
+    6272             :         }
+    6273             :       }
+    6274             :     }
+    6275             :   }
+    6276             : 
+    6277           6 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6278             : }
+    6279             : 
+    6280             : //}
+    6281             : 
+    6282             : /* isOffboard() //{ */
+    6283             : 
+    6284          18 : bool ControlManager::isOffboard(void) {
+    6285             : 
+    6286          18 :   if (!sh_hw_api_status_.hasMsg()) {
+    6287           0 :     return false;
+    6288             :   }
+    6289             : 
+    6290          18 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6291             : 
+    6292          18 :   return hw_api_status->connected && hw_api_status->offboard;
+    6293             : }
+    6294             : 
+    6295             : //}
+    6296             : 
+    6297             : /* setCallbacks() //{ */
+    6298             : 
+    6299          94 : void ControlManager::setCallbacks(bool in) {
+    6300             : 
+    6301          94 :   callbacks_enabled_ = in;
+    6302             : 
+    6303          94 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6304          94 :   req_enable_callbacks.data = callbacks_enabled_;
+    6305             : 
+    6306             :   {
+    6307         188 :     std::scoped_lock lock(mutex_tracker_list_);
+    6308             : 
+    6309             :     // set callbacks to all trackers
+    6310         658 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6311         564 :       tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6312             :     }
+    6313             :   }
+    6314          94 : }
+    6315             : 
+    6316             : //}
+    6317             : 
+    6318             : /* publishDiagnostics() //{ */
+    6319             : 
+    6320       18869 : void ControlManager::publishDiagnostics(void) {
+    6321             : 
+    6322       18869 :   if (!is_initialized_) {
+    6323           0 :     return;
+    6324             :   }
+    6325             : 
+    6326       56607 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6327       56607 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6328             : 
+    6329       37738 :   std::scoped_lock lock(mutex_diagnostics_);
+    6330             : 
+    6331       37738 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6332             : 
+    6333       18869 :   diagnostics_msg.stamp    = ros::Time::now();
+    6334       18869 :   diagnostics_msg.uav_name = _uav_name_;
+    6335             : 
+    6336       18869 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6337             : 
+    6338       18869 :   diagnostics_msg.output_enabled = output_enabled_;
+    6339             : 
+    6340       18869 :   diagnostics_msg.joystick_active = rc_goto_active_;
+    6341             : 
+    6342             :   {
+    6343       18869 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6344             : 
+    6345       18869 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6346             :   }
+    6347             : 
+    6348       18869 :   diagnostics_msg.bumper_active = bumper_repulsing_;
+    6349             : 
+    6350             :   // | ----------------- fill the tracker status ---------------- |
+    6351             : 
+    6352             :   {
+    6353       37738 :     std::scoped_lock lock(mutex_tracker_list_);
+    6354             : 
+    6355       18869 :     mrs_msgs::TrackerStatus tracker_status;
+    6356             : 
+    6357       18869 :     diagnostics_msg.active_tracker = _tracker_names_.at(active_tracker_idx_);
+    6358       18869 :     diagnostics_msg.tracker_status = tracker_list_.at(active_tracker_idx_)->getStatus();
+    6359             :   }
+    6360             : 
+    6361             :   // | --------------- fill the controller status --------------- |
+    6362             : 
+    6363             :   {
+    6364       37738 :     std::scoped_lock lock(mutex_controller_list_);
+    6365             : 
+    6366       18869 :     mrs_msgs::ControllerStatus controller_status;
+    6367             : 
+    6368       18869 :     diagnostics_msg.active_controller = _controller_names_.at(active_controller_idx_);
+    6369       18869 :     diagnostics_msg.controller_status = controller_list_.at(active_controller_idx_)->getStatus();
+    6370             :   }
+    6371             : 
+    6372             :   // | ------------ fill in the available controllers ----------- |
+    6373             : 
+    6374      113214 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6375       94345 :     if ((_controller_names_.at(i) != _failsafe_controller_name_) && (_controller_names_.at(i) != _eland_controller_name_)) {
+    6376       56607 :       diagnostics_msg.available_controllers.push_back(_controller_names_.at(i));
+    6377       56607 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_.at(i)).human_switchable);
+    6378             :     }
+    6379             :   }
+    6380             : 
+    6381             :   // | ------------- fill in the available trackers ------------- |
+    6382             : 
+    6383      132295 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6384      113426 :     if (_tracker_names_.at(i) != _null_tracker_name_) {
+    6385       94557 :       diagnostics_msg.available_trackers.push_back(_tracker_names_.at(i));
+    6386       94557 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_.at(i)).human_switchable);
+    6387             :     }
+    6388             :   }
+    6389             : 
+    6390             :   // | ------------------------- publish ------------------------ |
+    6391             : 
+    6392       18869 :   ph_diagnostics_.publish(diagnostics_msg);
+    6393             : }
+    6394             : 
+    6395             : //}
+    6396             : 
+    6397             : /* setConstraintsToTrackers() //{ */
+    6398             : 
+    6399         375 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6400             : 
+    6401        1125 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6402        1125 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6403             : 
+    6404         375 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6405             : 
+    6406             :   {
+    6407         750 :     std::scoped_lock lock(mutex_tracker_list_);
+    6408             : 
+    6409             :     // for each tracker
+    6410        2628 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6411             : 
+    6412             :       // if it is the active one, update and retrieve the command
+    6413        6759 :       response = tracker_list_.at(i)->setConstraints(
+    6414        6759 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6415             :     }
+    6416             :   }
+    6417         375 : }
+    6418             : 
+    6419             : //}
+    6420             : 
+    6421             : /* setConstraintsToControllers() //{ */
+    6422             : 
+    6423         424 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6424             : 
+    6425        1272 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6426        1272 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6427             : 
+    6428         424 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6429             : 
+    6430             :   {
+    6431         848 :     std::scoped_lock lock(mutex_controller_list_);
+    6432             : 
+    6433             :     // for each controller
+    6434        2544 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6435             : 
+    6436             :       // if it is the active one, update and retrieve the command
+    6437        6360 :       response = controller_list_.at(i)->setConstraints(
+    6438        6360 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6439             :     }
+    6440             :   }
+    6441         424 : }
+    6442             : 
+    6443             : //}
+    6444             : 
+    6445             : /* setConstraints() //{ */
+    6446             : 
+    6447         107 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6448             : 
+    6449         321 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6450         321 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6451             : 
+    6452         107 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6453             : 
+    6454         107 :   setConstraintsToTrackers(constraints);
+    6455             : 
+    6456         107 :   setConstraintsToControllers(constraints);
+    6457         107 : }
+    6458             : 
+    6459             : //}
+    6460             : 
+    6461             : /* enforceControllerConstraints() //{ */
+    6462             : 
+    6463      135422 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6464             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6465             : 
+    6466             :   // copy member variables
+    6467      270844 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6468             : 
+    6469      135422 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6470      116468 :     return {};
+    6471             :   }
+    6472             : 
+    6473       18954 :   bool enforcing = false;
+    6474             : 
+    6475       18954 :   auto constraints_out = constraints;
+    6476             : 
+    6477       37908 :   std::scoped_lock lock(mutex_tracker_list_);
+    6478             : 
+    6479             :   // enforce horizontal speed
+    6480       18954 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6481       13671 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6482             : 
+    6483       13671 :     enforcing = true;
+    6484             :   }
+    6485             : 
+    6486             :   // enforce horizontal acceleration
+    6487       18954 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6488       17317 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6489             : 
+    6490       17317 :     enforcing = true;
+    6491             :   }
+    6492             : 
+    6493             :   // enforce vertical ascending speed
+    6494       18954 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6495       13671 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6496             : 
+    6497       13671 :     enforcing = true;
+    6498             :   }
+    6499             : 
+    6500             :   // enforce vertical ascending acceleration
+    6501       18954 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6502           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6503             : 
+    6504           0 :     enforcing = true;
+    6505             :   }
+    6506             : 
+    6507             :   // enforce vertical descending speed
+    6508       18954 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6509       13671 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6510             : 
+    6511       13671 :     enforcing = true;
+    6512             :   }
+    6513             : 
+    6514             :   // enforce vertical descending acceleration
+    6515       18954 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6516           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6517             : 
+    6518           0 :     enforcing = true;
+    6519             :   }
+    6520             : 
+    6521       18954 :   if (enforcing) {
+    6522       17317 :     return {constraints_out};
+    6523             :   } else {
+    6524        1637 :     return {};
+    6525             :   }
+    6526             : }
+    6527             : 
+    6528             : //}
+    6529             : 
+    6530             : /* isFlyingNormally() //{ */
+    6531             : 
+    6532       19134 : bool ControlManager::isFlyingNormally(void) {
+    6533             : 
+    6534       16486 :   return callbacks_enabled_ && (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6535       10378 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6536       10378 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6537       37814 :           _controller_names_.size() == 1) &&
+    6538       27318 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6539             : }
+    6540             : 
+    6541             : //}
+    6542             : 
+    6543             : /* //{ getMass() */
+    6544             : 
+    6545         551 : double ControlManager::getMass(void) {
+    6546             : 
+    6547        1102 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6548             : 
+    6549         551 :   if (last_control_output.diagnostics.mass_estimator) {
+    6550          13 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6551             :   } else {
+    6552         538 :     return _uav_mass_;
+    6553             :   }
+    6554             : }
+    6555             : 
+    6556             : //}
+    6557             : 
+    6558             : /* loadConfigFile() //{ */
+    6559             : 
+    6560           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6561             : 
+    6562           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6563             : 
+    6564           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6565             : 
+    6566             :   // load the user-requested file
+    6567             :   {
+    6568           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6569           0 :     int         result  = std::system(command.c_str());
+    6570             : 
+    6571           0 :     if (result != 0) {
+    6572           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6573           0 :       return false;
+    6574             :     }
+    6575             :   }
+    6576             : 
+    6577             :   // load the platform config
+    6578           0 :   if (_platform_config_ != "") {
+    6579           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6580           0 :     int         result  = std::system(command.c_str());
+    6581             : 
+    6582           0 :     if (result != 0) {
+    6583           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6584           0 :       return false;
+    6585             :     }
+    6586             :   }
+    6587             : 
+    6588             :   // load the custom config
+    6589           0 :   if (_custom_config_ != "") {
+    6590           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6591           0 :     int         result  = std::system(command.c_str());
+    6592             : 
+    6593           0 :     if (result != 0) {
+    6594           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6595           0 :       return false;
+    6596             :     }
+    6597             :   }
+    6598             : 
+    6599           0 :   return true;
+    6600             : }
+    6601             : 
+    6602             : //}
+    6603             : 
+    6604             : // | ----------------------- safety area ---------------------- |
+    6605             : 
+    6606             : /* //{ isPointInSafetyArea3d() */
+    6607             : 
+    6608        1714 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6609             : 
+    6610        1714 :   if (!use_safety_area_) {
+    6611         661 :     return true;
+    6612             :   }
+    6613             : 
+    6614        2106 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6615             : 
+    6616        1053 :   if (!tfed_horizontal) {
+    6617           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6618           0 :     return false;
+    6619             :   }
+    6620             : 
+    6621        1053 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6622           3 :     return false;
+    6623             :   }
+    6624             : 
+    6625        1050 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6626           3 :     return false;
+    6627             :   }
+    6628             : 
+    6629        1047 :   return true;
+    6630             : }
+    6631             : 
+    6632             : //}
+    6633             : 
+    6634             : /* //{ isPointInSafetyArea2d() */
+    6635             : 
+    6636        9703 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6637             : 
+    6638        9703 :   if (!use_safety_area_) {
+    6639         591 :     return true;
+    6640             :   }
+    6641             : 
+    6642       18224 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6643             : 
+    6644        9112 :   if (!tfed_horizontal) {
+    6645           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6646           0 :     return false;
+    6647             :   }
+    6648             : 
+    6649        9112 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6650          75 :     return false;
+    6651             :   }
+    6652             : 
+    6653        9037 :   return true;
+    6654             : }
+    6655             : 
+    6656             : //}
+    6657             : 
+    6658             : /* //{ isPathToPointInSafetyArea3d() */
+    6659             : 
+    6660         806 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6661             : 
+    6662         806 :   if (!use_safety_area_) {
+    6663         661 :     return true;
+    6664             :   }
+    6665             : 
+    6666         145 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6667           2 :     return false;
+    6668             :   }
+    6669             : 
+    6670         286 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6671             : 
+    6672             :   {
+    6673         143 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6674             : 
+    6675         143 :     if (!ret) {
+    6676             : 
+    6677           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6678             : 
+    6679           0 :       return false;
+    6680             :     }
+    6681             : 
+    6682         143 :     start_transformed = ret.value();
+    6683             :   }
+    6684             : 
+    6685             :   {
+    6686         143 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6687             : 
+    6688         143 :     if (!ret) {
+    6689             : 
+    6690           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6691             : 
+    6692           0 :       return false;
+    6693             :     }
+    6694             : 
+    6695         143 :     end_transformed = ret.value();
+    6696             :   }
+    6697             : 
+    6698         143 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6699         143 :                                    end_transformed.reference.position.y);
+    6700             : }
+    6701             : 
+    6702             : //}
+    6703             : 
+    6704             : /* //{ isPathToPointInSafetyArea2d() */
+    6705             : 
+    6706          31 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6707             : 
+    6708          31 :   if (!use_safety_area_) {
+    6709           0 :     return true;
+    6710             :   }
+    6711             : 
+    6712          62 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6713             : 
+    6714          31 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6715           0 :     return false;
+    6716             :   }
+    6717             : 
+    6718             :   {
+    6719          31 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6720             : 
+    6721          31 :     if (!ret) {
+    6722             : 
+    6723           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6724             : 
+    6725           0 :       return false;
+    6726             :     }
+    6727             : 
+    6728          31 :     start_transformed = ret.value();
+    6729             :   }
+    6730             : 
+    6731             :   {
+    6732          31 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6733             : 
+    6734          31 :     if (!ret) {
+    6735             : 
+    6736           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6737             : 
+    6738           0 :       return false;
+    6739             :     }
+    6740             : 
+    6741          31 :     end_transformed = ret.value();
+    6742             :   }
+    6743             : 
+    6744          31 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6745          31 :                                    end_transformed.reference.position.y);
+    6746             : }
+    6747             : 
+    6748             : //}
+    6749             : 
+    6750             : /* //{ getMaxZ() */
+    6751             : 
+    6752       13294 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6753             : 
+    6754             :   // | ------- first, calculate max_z from the safety area ------ |
+    6755             : 
+    6756       13294 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6757             : 
+    6758             :   {
+    6759             : 
+    6760       26588 :     geometry_msgs::PointStamped point;
+    6761             : 
+    6762       13294 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6763       13294 :     point.point.x         = 0;
+    6764       13294 :     point.point.y         = 0;
+    6765       13294 :     point.point.z         = _safety_area_max_z_;
+    6766             : 
+    6767       13294 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6768             : 
+    6769       13294 :     if (!ret) {
+    6770           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6771             :     }
+    6772             : 
+    6773       13294 :     safety_area_max_z = ret->point.z;
+    6774             :   }
+    6775             : 
+    6776             :   // | ------------ overwrite from estimation manager ----------- |
+    6777             : 
+    6778       13294 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6779             : 
+    6780             :   {
+    6781             :     // if possible, override it with max z from the estimation manager
+    6782       13294 :     if (sh_max_z_.hasMsg()) {
+    6783             : 
+    6784       26558 :       auto msg = sh_max_z_.getMsg();
+    6785             : 
+    6786             :       // transform it into the safety area frame
+    6787       26558 :       geometry_msgs::PointStamped point;
+    6788       13279 :       point.header  = msg->header;
+    6789       13279 :       point.point.x = 0;
+    6790       13279 :       point.point.y = 0;
+    6791       13279 :       point.point.z = msg->value;
+    6792             : 
+    6793       13279 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6794             : 
+    6795       13279 :       if (!ret) {
+    6796           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6797             :       }
+    6798             : 
+    6799       13279 :       estimation_manager_max_z = ret->point.z;
+    6800             :     }
+    6801             :   }
+    6802             : 
+    6803       13294 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6804       12495 :     return estimation_manager_max_z;
+    6805             :   } else {
+    6806         799 :     return safety_area_max_z;
+    6807             :   }
+    6808             : }
+    6809             : 
+    6810             : //}
+    6811             : 
+    6812             : /* //{ getMinZ() */
+    6813             : 
+    6814       13297 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6815             : 
+    6816       13297 :   if (!use_safety_area_) {
+    6817           0 :     return std::numeric_limits<double>::lowest();
+    6818             :   }
+    6819             : 
+    6820       26594 :   geometry_msgs::PointStamped point;
+    6821             : 
+    6822       13297 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6823       13297 :   point.point.x         = 0;
+    6824       13297 :   point.point.y         = 0;
+    6825       13297 :   point.point.z         = _safety_area_min_z_;
+    6826             : 
+    6827       26594 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6828             : 
+    6829       13297 :   if (!ret) {
+    6830           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6831           0 :     return std::numeric_limits<double>::lowest();
+    6832             :   }
+    6833             : 
+    6834       13297 :   return ret->point.z;
+    6835             : }
+    6836             : 
+    6837             : //}
+    6838             : 
+    6839             : // | --------------------- obstacle bumper -------------------- |
+    6840             : 
+    6841             : /* bumperPushFromObstacle() //{ */
+    6842             : 
+    6843         259 : void ControlManager::bumperPushFromObstacle(void) {
+    6844             : 
+    6845             :   // | --------------- fabricate the min distances -------------- |
+    6846             : 
+    6847         259 :   double min_distance_horizontal = _bumper_horizontal_distance_;
+    6848         259 :   double min_distance_vertical   = _bumper_vertical_distance_;
+    6849             : 
+    6850         259 :   if (_bumper_horizontal_derive_from_dynamics_ || _bumper_vertical_derive_from_dynamics_) {
+    6851             : 
+    6852         259 :     auto constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    6853             : 
+    6854         259 :     if (_bumper_horizontal_derive_from_dynamics_) {
+    6855             : 
+    6856         259 :       const double horizontal_t_stop    = constraints.constraints.horizontal_speed / constraints.constraints.horizontal_acceleration;
+    6857         259 :       const double horizontal_stop_dist = (horizontal_t_stop * constraints.constraints.horizontal_speed) / 2.0;
+    6858             : 
+    6859         259 :       min_distance_horizontal += 1.5 * horizontal_stop_dist;
+    6860             :     }
+    6861             : 
+    6862         259 :     if (_bumper_vertical_derive_from_dynamics_) {
+    6863             : 
+    6864             : 
+    6865             :       // larger from the two accelerations
+    6866         518 :       const double vert_acc = constraints.constraints.vertical_ascending_acceleration > constraints.constraints.vertical_descending_acceleration
+    6867         259 :                                   ? constraints.constraints.vertical_ascending_acceleration
+    6868             :                                   : constraints.constraints.vertical_descending_acceleration;
+    6869             : 
+    6870             :       // larger from the two speeds
+    6871         518 :       const double vert_speed = constraints.constraints.vertical_ascending_speed > constraints.constraints.vertical_descending_speed
+    6872         259 :                                     ? constraints.constraints.vertical_ascending_speed
+    6873             :                                     : constraints.constraints.vertical_descending_speed;
+    6874             : 
+    6875         259 :       const double vertical_t_stop    = vert_speed / vert_acc;
+    6876         259 :       const double vertical_stop_dist = (vertical_t_stop * vert_speed) / 2.0;
+    6877             : 
+    6878         259 :       min_distance_vertical += 1.5 * vertical_stop_dist;
+    6879             :     }
+    6880             :   }
+    6881             : 
+    6882             :   // | ----------------------------  ---------------------------- |
+    6883             : 
+    6884             :   // copy member variables
+    6885         259 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6886         259 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6887             : 
+    6888         259 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6889             : 
+    6890         259 :   double direction          = 0;
+    6891         259 :   double repulsion_distance = std::numeric_limits<double>::max();
+    6892             : 
+    6893         259 :   bool horizontal_collision_detected = false;
+    6894         259 :   bool vertical_collision_detected   = false;
+    6895             : 
+    6896         259 :   double min_horizontal_sector_distance = std::numeric_limits<double>::max();
+    6897         259 :   size_t min_sector_id                  = 0;
+    6898             : 
+    6899        2331 :   for (unsigned long i = 0; i < bumper_data->n_horizontal_sectors; i++) {
+    6900             : 
+    6901        2072 :     if (bumper_data->sectors.at(i) < 0) {
+    6902           0 :       continue;
+    6903             :     }
+    6904             : 
+    6905        2072 :     if (bumper_data->sectors.at(i) < min_horizontal_sector_distance) {
+    6906         259 :       min_horizontal_sector_distance = bumper_data->sectors.at(i);
+    6907         259 :       min_sector_id                  = i;
+    6908             :     }
+    6909             :   }
+    6910             : 
+    6911             :   // if the sector is under the threshold distance
+    6912         259 :   if (min_horizontal_sector_distance < min_distance_horizontal) {
+    6913             : 
+    6914             :     // get the desired direction of motion
+    6915         104 :     double oposite_direction  = double(min_sector_id) * sector_size + M_PI;
+    6916         104 :     int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6917             : 
+    6918             :     // get the id of the oposite sector
+    6919         104 :     direction = oposite_direction;
+    6920             : 
+    6921         104 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %lu vs. %d), obstacle distance: %.2f, repulsing", min_sector_id,
+    6922             :                       oposite_sector_idx, bumper_data->sectors.at(min_sector_id));
+    6923             : 
+    6924         104 :     repulsion_distance = min_distance_horizontal + _bumper_horizontal_overshoot_ - bumper_data->sectors.at(min_sector_id);
+    6925             : 
+    6926         104 :     horizontal_collision_detected = true;
+    6927             :   }
+    6928             : 
+    6929         259 :   double vertical_repulsion_distance = 0;
+    6930             : 
+    6931             :   // check for vertical collision down
+    6932         259 :   if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors) > 0 && bumper_data->sectors.at(bumper_data->n_horizontal_sectors) <= min_distance_vertical) {
+    6933             : 
+    6934           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    6935           0 :     vertical_collision_detected = true;
+    6936           0 :     vertical_repulsion_distance = min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors);
+    6937             :   }
+    6938             : 
+    6939             :   // check for vertical collision up
+    6940         518 :   if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) > 0 &&
+    6941         259 :       bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) <= min_distance_vertical) {
+    6942             : 
+    6943           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    6944           0 :     vertical_collision_detected = true;
+    6945           0 :     vertical_repulsion_distance = -(min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1));
+    6946             :   }
+    6947             : 
+    6948             :   // if potential collision was detected and we should start the repulsing_
+    6949         259 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    6950             : 
+    6951         104 :     if (!bumper_repulsing_) {
+    6952             : 
+    6953           1 :       if (_bumper_switch_tracker_) {
+    6954             : 
+    6955           1 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6956           2 :         std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+    6957             : 
+    6958             :         // remember the previously active tracker
+    6959           1 :         bumper_previous_tracker_ = active_tracker_name;
+    6960             : 
+    6961           1 :         if (active_tracker_name != _bumper_tracker_name_) {
+    6962             : 
+    6963           0 :           switchTracker(_bumper_tracker_name_);
+    6964             :         }
+    6965             :       }
+    6966             : 
+    6967           1 :       if (_bumper_switch_controller_) {
+    6968             : 
+    6969           1 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6970           2 :         std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    6971             : 
+    6972             :         // remember the previously active controller
+    6973           1 :         bumper_previous_controller_ = active_controller_name;
+    6974             : 
+    6975           1 :         if (active_controller_name != _bumper_controller_name_) {
+    6976             : 
+    6977           1 :           switchController(_bumper_controller_name_);
+    6978             :         }
+    6979             :       }
+    6980             :     }
+    6981             : 
+    6982         104 :     bumper_repulsing_ = true;
+    6983             : 
+    6984         104 :     callbacks_enabled_ = false;
+    6985             : 
+    6986           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    6987             : 
+    6988         104 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6989             : 
+    6990             :     // create the reference in the fcu_untilted frame
+    6991         104 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    6992             : 
+    6993         104 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    6994             : 
+    6995         104 :     if (horizontal_collision_detected) {
+    6996         104 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    6997         104 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    6998             :     } else {
+    6999           0 :       reference_fcu_untilted.reference.position.x = 0;
+    7000           0 :       reference_fcu_untilted.reference.position.y = 0;
+    7001             :     }
+    7002             : 
+    7003         104 :     reference_fcu_untilted.reference.heading = 0;
+    7004             : 
+    7005         104 :     if (vertical_collision_detected) {
+    7006           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    7007             :     } else {
+    7008         104 :       reference_fcu_untilted.reference.position.z = 0;
+    7009             :     }
+    7010             : 
+    7011             :     {
+    7012         104 :       std::scoped_lock lock(mutex_tracker_list_);
+    7013             : 
+    7014             :       // transform the reference into the currently used frame
+    7015             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    7016             :       // to the tracker before we actually call the goto service
+    7017             : 
+    7018         104 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    7019             : 
+    7020         104 :       if (!ret) {
+    7021           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    7022           0 :         return;
+    7023             :       }
+    7024             : 
+    7025         104 :       reference_fcu_untilted = ret.value();
+    7026             : 
+    7027             :       // copy the reference into the service type message
+    7028         104 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    7029         104 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    7030             : 
+    7031             :       // disable callbacks of all trackers
+    7032         104 :       req_enable_callbacks.data = false;
+    7033         728 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    7034         624 :         tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7035             :       }
+    7036             : 
+    7037             :       // enable the callbacks for the active tracker
+    7038         104 :       req_enable_callbacks.data = true;
+    7039         104 :       tracker_list_.at(active_tracker_idx_)
+    7040         104 :           ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7041             : 
+    7042             :       // call the goto
+    7043         104 :       tracker_response = tracker_list_.at(active_tracker_idx_)
+    7044         104 :                              ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    7045             : 
+    7046             :       // disable the callbacks back again
+    7047         104 :       req_enable_callbacks.data = false;
+    7048         104 :       tracker_list_.at(active_tracker_idx_)
+    7049         104 :           ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7050             :     }
+    7051             :   }
+    7052             : 
+    7053             :   // if repulsing_ and the distance is safe once again
+    7054         259 :   if (bumper_repulsing_ && !horizontal_collision_detected && !vertical_collision_detected) {
+    7055             : 
+    7056           1 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: no more collision, stopping repulsion");
+    7057             : 
+    7058           1 :     if (_bumper_switch_tracker_) {
+    7059             : 
+    7060           1 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7061           2 :       std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+    7062             : 
+    7063           1 :       if (active_tracker_name != bumper_previous_tracker_) {
+    7064             : 
+    7065           0 :         switchTracker(bumper_previous_tracker_);
+    7066             :       }
+    7067             :     }
+    7068             : 
+    7069           1 :     if (_bumper_switch_controller_) {
+    7070             : 
+    7071           1 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7072           2 :       std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7073             : 
+    7074           1 :       if (active_controller_name != bumper_previous_controller_) {
+    7075             : 
+    7076           1 :         switchController(bumper_previous_controller_);
+    7077             :       }
+    7078             :     }
+    7079             : 
+    7080           1 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    7081             : 
+    7082             :     {
+    7083           2 :       std::scoped_lock lock(mutex_tracker_list_);
+    7084             : 
+    7085             :       // enable callbacks of all trackers
+    7086           1 :       req_enable_callbacks.data = true;
+    7087           7 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    7088           6 :         tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7089             :       }
+    7090             :     }
+    7091             : 
+    7092           1 :     callbacks_enabled_ = true;
+    7093             : 
+    7094           1 :     bumper_repulsing_ = false;
+    7095             :   }
+    7096             : }
+    7097             : 
+    7098             : //}
+    7099             : 
+    7100             : /* bumperGetSectorId() //{ */
+    7101             : 
+    7102         104 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    7103             : 
+    7104             :   // copy member variables
+    7105         104 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    7106             : 
+    7107             :   // heading of the point in drone frame
+    7108         104 :   double point_heading_horizontal = atan2(y, x);
+    7109             : 
+    7110         104 :   point_heading_horizontal += TAU;
+    7111             : 
+    7112             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7113         104 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7114         104 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7115             :   }
+    7116             : 
+    7117             :   // heading of the right edge of the first sector
+    7118         104 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7119             : 
+    7120             :   // calculate the idx
+    7121         104 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7122             : 
+    7123         104 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7124           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7125             :   }
+    7126             : 
+    7127         208 :   return idx;
+    7128             : }
+    7129             : 
+    7130             : //}
+    7131             : 
+    7132             : // | ------------------------- safety ------------------------- |
+    7133             : 
+    7134             : /* //{ changeLandingState() */
+    7135             : 
+    7136          12 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7137             : 
+    7138             :   // copy member variables
+    7139          24 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7140             : 
+    7141             :   {
+    7142          12 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7143             : 
+    7144          12 :     previous_state_landing_ = current_state_landing_;
+    7145          12 :     current_state_landing_  = new_state;
+    7146             :   }
+    7147             : 
+    7148          12 :   switch (current_state_landing_) {
+    7149             : 
+    7150           4 :     case IDLE_STATE:
+    7151           4 :       break;
+    7152           8 :     case LANDING_STATE: {
+    7153             : 
+    7154           8 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7155           8 :       timer_eland_.start();
+    7156           8 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7157           8 :       eland_triggered_ = true;
+    7158           8 :       bumper_enabled_  = false;
+    7159             : 
+    7160           8 :       landing_uav_mass_ = getMass();
+    7161             :     }
+    7162             : 
+    7163           8 :     break;
+    7164             :   }
+    7165             : 
+    7166          12 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7167          12 : }
+    7168             : 
+    7169             : //}
+    7170             : 
+    7171             : /* hover() //{ */
+    7172             : 
+    7173           1 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7174             : 
+    7175           1 :   if (!is_initialized_) {
+    7176           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7177             :   }
+    7178             : 
+    7179           1 :   if (eland_triggered_) {
+    7180           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7181             :   }
+    7182             : 
+    7183           1 :   if (failsafe_triggered_) {
+    7184           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7185             :   }
+    7186             : 
+    7187             :   {
+    7188           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7189             : 
+    7190           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7191           1 :     std_srvs::TriggerRequest            request;
+    7192             : 
+    7193           1 :     response = tracker_list_.at(active_tracker_idx_)->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7194             : 
+    7195           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7196             : 
+    7197           2 :       return std::tuple(response->success, response->message);
+    7198             : 
+    7199             :     } else {
+    7200             : 
+    7201           0 :       std::stringstream ss;
+    7202           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'hover()' function!";
+    7203             : 
+    7204           0 :       return std::tuple(false, ss.str());
+    7205             :     }
+    7206             :   }
+    7207             : }
+    7208             : 
+    7209             : //}
+    7210             : 
+    7211             : /* //{ ehover() */
+    7212             : 
+    7213           4 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7214             : 
+    7215           4 :   if (!is_initialized_) {
+    7216           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7217             :   }
+    7218             : 
+    7219           4 :   if (eland_triggered_) {
+    7220           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7221             :   }
+    7222             : 
+    7223           4 :   if (failsafe_triggered_) {
+    7224           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7225             :   }
+    7226             : 
+    7227             :   // copy the member variables
+    7228           8 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7229           4 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7230             : 
+    7231           4 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7232             : 
+    7233           0 :     std::stringstream ss;
+    7234           0 :     ss << "can not trigger ehover while not flying";
+    7235           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7236             : 
+    7237           0 :     return std::tuple(false, ss.str());
+    7238             :   }
+    7239             : 
+    7240           4 :   ungripSrv();
+    7241             : 
+    7242             :   {
+    7243             : 
+    7244           4 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7245             : 
+    7246             :     // check if the tracker was successfully switched
+    7247             :     // this is vital, that is the core of the hover
+    7248           4 :     if (!success) {
+    7249             : 
+    7250           0 :       std::stringstream ss;
+    7251           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7252           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7253             : 
+    7254           0 :       return std::tuple(success, ss.str());
+    7255             :     }
+    7256             :   }
+    7257             : 
+    7258             :   {
+    7259           8 :     auto [success, message] = switchController(_eland_controller_name_);
+    7260             : 
+    7261             :     // check if the controller was successfully switched
+    7262             :     // this is not vital, we can continue without that
+    7263           4 :     if (!success) {
+    7264             : 
+    7265           0 :       std::stringstream ss;
+    7266           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7267           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7268             :     }
+    7269             :   }
+    7270             : 
+    7271           4 :   std::stringstream ss;
+    7272           4 :   ss << "ehover activated";
+    7273           4 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7274             : 
+    7275           4 :   callbacks_enabled_ = false;
+    7276             : 
+    7277           4 :   return std::tuple(true, ss.str());
+    7278             : }
+    7279             : 
+    7280             : //}
+    7281             : 
+    7282             : /* eland() //{ */
+    7283             : 
+    7284           8 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7285             : 
+    7286           8 :   if (!is_initialized_) {
+    7287           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7288             :   }
+    7289             : 
+    7290           8 :   if (eland_triggered_) {
+    7291           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7292             :   }
+    7293             : 
+    7294           8 :   if (failsafe_triggered_) {
+    7295           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7296             :   }
+    7297             : 
+    7298             :   // copy member variables
+    7299          16 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7300           8 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7301             : 
+    7302           8 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7303             : 
+    7304           0 :     std::stringstream ss;
+    7305           0 :     ss << "can not trigger eland while not flying";
+    7306           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7307             : 
+    7308           0 :     return std::tuple(false, ss.str());
+    7309             :   }
+    7310             : 
+    7311           8 :   if (_rc_emergency_handoff_) {
+    7312             : 
+    7313           0 :     toggleOutput(false);
+    7314             : 
+    7315           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7316             :   }
+    7317             : 
+    7318             :   {
+    7319           8 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7320             : 
+    7321             :     // check if the tracker was successfully switched
+    7322             :     // this is vital
+    7323           8 :     if (!success) {
+    7324             : 
+    7325           0 :       std::stringstream ss;
+    7326           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7327           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7328             : 
+    7329           0 :       return std::tuple(success, ss.str());
+    7330             :     }
+    7331             :   }
+    7332             : 
+    7333             :   {
+    7334          16 :     auto [success, message] = switchController(_eland_controller_name_);
+    7335             : 
+    7336             :     // check if the controller was successfully switched
+    7337             :     // this is not vital, we can continue without it
+    7338           8 :     if (!success) {
+    7339             : 
+    7340           0 :       std::stringstream ss;
+    7341           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7342           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7343             :     }
+    7344             :   }
+    7345             : 
+    7346             :   // | ----------------- call the eland service ----------------- |
+    7347             : 
+    7348           8 :   std::stringstream ss;
+    7349             :   bool              success;
+    7350             : 
+    7351           8 :   if (elandSrv()) {
+    7352             : 
+    7353           8 :     changeLandingState(LANDING_STATE);
+    7354             : 
+    7355           8 :     odometryCallbacksSrv(false);
+    7356             : 
+    7357           8 :     ss << "eland activated";
+    7358           8 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7359             : 
+    7360           8 :     success = true;
+    7361             : 
+    7362           8 :     callbacks_enabled_ = false;
+    7363             : 
+    7364             :   } else {
+    7365             : 
+    7366           0 :     ss << "error during activation of eland";
+    7367           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7368             : 
+    7369           0 :     success = false;
+    7370             :   }
+    7371             : 
+    7372          16 :   return std::tuple(success, ss.str());
+    7373             : }
+    7374             : 
+    7375             : //}
+    7376             : 
+    7377             : /* failsafe() //{ */
+    7378             : 
+    7379           8 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7380             : 
+    7381             :   // copy member variables
+    7382          16 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7383           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7384           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7385             : 
+    7386           8 :   if (!is_initialized_) {
+    7387           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7388             :   }
+    7389             : 
+    7390           8 :   if (failsafe_triggered_) {
+    7391           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7392             :   }
+    7393             : 
+    7394           8 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7395             : 
+    7396           0 :     std::stringstream ss;
+    7397           0 :     ss << "can not trigger failsafe while not flying";
+    7398           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7399           0 :     return std::tuple(false, ss.str());
+    7400             :   }
+    7401             : 
+    7402           8 :   if (_rc_emergency_handoff_) {
+    7403             : 
+    7404           0 :     toggleOutput(false);
+    7405             : 
+    7406           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7407             :   }
+    7408             : 
+    7409           8 :   if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+    7410           0 :     return eland();
+    7411             :   }
+    7412             : 
+    7413           8 :   if (_parachute_enabled_) {
+    7414             : 
+    7415           0 :     auto [success, message] = deployParachute();
+    7416             : 
+    7417           0 :     if (success) {
+    7418             : 
+    7419           0 :       std::stringstream ss;
+    7420           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7421           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7422             : 
+    7423           0 :       return std::tuple(true, ss.str());
+    7424             : 
+    7425             :     } else {
+    7426             : 
+    7427           0 :       std::stringstream ss;
+    7428           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7429           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7430             :     }
+    7431             :   }
+    7432             : 
+    7433           8 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7434             : 
+    7435             :     try {
+    7436             : 
+    7437          16 :       std::scoped_lock lock(mutex_controller_list_);
+    7438             : 
+    7439           8 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7440           8 :       controller_list_.at(_failsafe_controller_idx_)->activate(last_control_output);
+    7441             : 
+    7442             :       {
+    7443          16 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7444             : 
+    7445             :         // update the time (used in failsafe)
+    7446           8 :         controller_tracker_switch_time_ = ros::Time::now();
+    7447             :       }
+    7448             : 
+    7449           8 :       failsafe_triggered_ = true;
+    7450           8 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7451           8 :       timer_eland_.stop();
+    7452           8 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7453             : 
+    7454           8 :       landing_uav_mass_ = getMass();
+    7455             : 
+    7456           8 :       eland_triggered_ = false;
+    7457           8 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7458           8 :       timer_failsafe_.start();
+    7459           8 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7460             : 
+    7461           8 :       bumper_enabled_ = false;
+    7462             : 
+    7463           8 :       odometryCallbacksSrv(false);
+    7464             : 
+    7465           8 :       callbacks_enabled_ = false;
+    7466             : 
+    7467           8 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7468             : 
+    7469             :       // super important, switch the active controller idx
+    7470             :       try {
+    7471           8 :         controller_list_.at(active_controller_idx_)->deactivate();
+    7472           8 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7473             :       }
+    7474           0 :       catch (std::runtime_error& exrun) {
+    7475           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+    7476             :       }
+    7477             :     }
+    7478           0 :     catch (std::runtime_error& exrun) {
+    7479           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7480           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7481             :     }
+    7482             :   }
+    7483             : 
+    7484          16 :   return std::tuple(true, "failsafe activated");
+    7485             : }
+    7486             : 
+    7487             : //}
+    7488             : 
+    7489             : /* escalatingFailsafe() //{ */
+    7490             : 
+    7491         151 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7492             : 
+    7493         302 :   std::stringstream ss;
+    7494             : 
+    7495         151 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7496             : 
+    7497         143 :     ss << "too soon for escalating failsafe";
+    7498         143 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7499             : 
+    7500         143 :     return std::tuple(false, ss.str());
+    7501             :   }
+    7502             : 
+    7503           8 :   if (!output_enabled_) {
+    7504             : 
+    7505           0 :     ss << "not escalating failsafe, output is disabled";
+    7506           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7507             : 
+    7508           0 :     return std::tuple(false, ss.str());
+    7509             :   }
+    7510             : 
+    7511           8 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7512             : 
+    7513           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7514           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7515             : 
+    7516          16 :   std::string active_tracker_name    = _tracker_names_.at(active_tracker_idx);
+    7517          16 :   std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7518             : 
+    7519           8 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7520             : 
+    7521           8 :   escalating_failsafe_time_ = ros::Time::now();
+    7522             : 
+    7523           8 :   switch (next_state) {
+    7524             : 
+    7525           0 :     case ESC_NONE_STATE: {
+    7526             : 
+    7527           0 :       ss << "escalating failsafe has run to impossible situation";
+    7528           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7529             : 
+    7530           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7531             : 
+    7532             :       break;
+    7533             :     }
+    7534             : 
+    7535           2 :     case ESC_EHOVER_STATE: {
+    7536             : 
+    7537           2 :       ss << "escalating failsafe escalates to ehover";
+    7538           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7539             : 
+    7540           4 :       auto [success, message] = ehover();
+    7541             : 
+    7542           2 :       if (success) {
+    7543           2 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7544             :       }
+    7545             : 
+    7546           2 :       return {success, message};
+    7547             : 
+    7548             :       break;
+    7549             :     }
+    7550             : 
+    7551           2 :     case ESC_ELAND_STATE: {
+    7552             : 
+    7553           2 :       ss << "escalating failsafe escalates to eland";
+    7554           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7555             : 
+    7556           4 :       auto [success, message] = eland();
+    7557             : 
+    7558           2 :       if (success) {
+    7559           2 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7560             :       }
+    7561             : 
+    7562           2 :       return {success, message};
+    7563             : 
+    7564             :       break;
+    7565             :     }
+    7566             : 
+    7567           2 :     case ESC_FAILSAFE_STATE: {
+    7568             : 
+    7569           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7570             : 
+    7571           2 :       ss << "escalating failsafe escalates to failsafe";
+    7572           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7573             : 
+    7574           4 :       auto [success, message] = failsafe();
+    7575             : 
+    7576           2 :       if (success) {
+    7577           2 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7578             :       }
+    7579             : 
+    7580           2 :       return {success, message};
+    7581             : 
+    7582             :       break;
+    7583             :     }
+    7584             : 
+    7585           2 :     case ESC_FINISHED_STATE: {
+    7586             : 
+    7587           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7588             : 
+    7589           2 :       ss << "escalating failsafe has nothing more to do";
+    7590           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7591             : 
+    7592           4 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7593             : 
+    7594             :       break;
+    7595             :     }
+    7596             : 
+    7597           0 :     default: {
+    7598             : 
+    7599           0 :       break;
+    7600             :     }
+    7601             :   }
+    7602             : 
+    7603           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7604             : 
+    7605           0 :   return std::tuple(false, "escalating failsafe exception");
+    7606             : }
+    7607             : 
+    7608             : //}
+    7609             : 
+    7610             : /* getNextEscFailsafeState() //{ */
+    7611             : 
+    7612           8 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7613             : 
+    7614           8 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7615             : 
+    7616           8 :   switch (current_state) {
+    7617             : 
+    7618           2 :     case ESC_FINISHED_STATE: {
+    7619             : 
+    7620           2 :       return ESC_FINISHED_STATE;
+    7621             : 
+    7622             :       break;
+    7623             :     }
+    7624             : 
+    7625           2 :     case ESC_NONE_STATE: {
+    7626             : 
+    7627           2 :       if (_escalating_failsafe_ehover_) {
+    7628           2 :         return ESC_EHOVER_STATE;
+    7629           0 :       } else if (_escalating_failsafe_eland_) {
+    7630           0 :         return ESC_ELAND_STATE;
+    7631           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7632           0 :         return ESC_FAILSAFE_STATE;
+    7633             :       } else {
+    7634           0 :         return ESC_FINISHED_STATE;
+    7635             :       }
+    7636             : 
+    7637             :       break;
+    7638             :     }
+    7639             : 
+    7640           2 :     case ESC_EHOVER_STATE: {
+    7641             : 
+    7642           2 :       if (_escalating_failsafe_eland_) {
+    7643           2 :         return ESC_ELAND_STATE;
+    7644           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7645           0 :         return ESC_FAILSAFE_STATE;
+    7646             :       } else {
+    7647           0 :         return ESC_FINISHED_STATE;
+    7648             :       }
+    7649             : 
+    7650             :       break;
+    7651             :     }
+    7652             : 
+    7653           2 :     case ESC_ELAND_STATE: {
+    7654             : 
+    7655           2 :       if (_escalating_failsafe_failsafe_) {
+    7656           2 :         return ESC_FAILSAFE_STATE;
+    7657             :       } else {
+    7658           0 :         return ESC_FINISHED_STATE;
+    7659             :       }
+    7660             : 
+    7661             :       break;
+    7662             :     }
+    7663             : 
+    7664           0 :     case ESC_FAILSAFE_STATE: {
+    7665             : 
+    7666           0 :       return ESC_FINISHED_STATE;
+    7667             : 
+    7668             :       break;
+    7669             :     }
+    7670             :   }
+    7671             : 
+    7672           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7673             : 
+    7674           0 :   return ESC_NONE_STATE;
+    7675             : }
+    7676             : 
+    7677             : //}
+    7678             : 
+    7679             : // | ------------------- trajectory tracking ------------------ |
+    7680             : 
+    7681             : /* startTrajectoryTracking() //{ */
+    7682             : 
+    7683           2 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7684             : 
+    7685           2 :   if (!is_initialized_) {
+    7686           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7687             :   }
+    7688             : 
+    7689             :   {
+    7690           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    7691             : 
+    7692           2 :     std_srvs::TriggerResponse::ConstPtr response;
+    7693           2 :     std_srvs::TriggerRequest            request;
+    7694             : 
+    7695             :     response =
+    7696           2 :         tracker_list_.at(active_tracker_idx_)->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7697             : 
+    7698           2 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7699             : 
+    7700           4 :       return std::tuple(response->success, response->message);
+    7701             : 
+    7702             :     } else {
+    7703             : 
+    7704           0 :       std::stringstream ss;
+    7705           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'startTrajectoryTracking()' function!";
+    7706             : 
+    7707           0 :       return std::tuple(false, ss.str());
+    7708             :     }
+    7709             :   }
+    7710             : }
+    7711             : 
+    7712             : //}
+    7713             : 
+    7714             : /* stopTrajectoryTracking() //{ */
+    7715             : 
+    7716           1 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7717             : 
+    7718           1 :   if (!is_initialized_) {
+    7719           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7720             :   }
+    7721             : 
+    7722             :   {
+    7723           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7724             : 
+    7725           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7726           1 :     std_srvs::TriggerRequest            request;
+    7727             : 
+    7728             :     response =
+    7729           1 :         tracker_list_.at(active_tracker_idx_)->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7730             : 
+    7731           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7732             : 
+    7733           2 :       return std::tuple(response->success, response->message);
+    7734             : 
+    7735             :     } else {
+    7736             : 
+    7737           0 :       std::stringstream ss;
+    7738           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7739             : 
+    7740           0 :       return std::tuple(false, ss.str());
+    7741             :     }
+    7742             :   }
+    7743             : }
+    7744             : 
+    7745             : //}
+    7746             : 
+    7747             : /* resumeTrajectoryTracking() //{ */
+    7748             : 
+    7749           1 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7750             : 
+    7751           1 :   if (!is_initialized_) {
+    7752           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7753             :   }
+    7754             : 
+    7755             :   {
+    7756           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7757             : 
+    7758           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7759           1 :     std_srvs::TriggerRequest            request;
+    7760             : 
+    7761           1 :     response = tracker_list_.at(active_tracker_idx_)
+    7762           1 :                    ->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7763             : 
+    7764           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7765             : 
+    7766           2 :       return std::tuple(response->success, response->message);
+    7767             : 
+    7768             :     } else {
+    7769             : 
+    7770           0 :       std::stringstream ss;
+    7771           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7772             : 
+    7773           0 :       return std::tuple(false, ss.str());
+    7774             :     }
+    7775             :   }
+    7776             : }
+    7777             : 
+    7778             : //}
+    7779             : 
+    7780             : /* gotoTrajectoryStart() //{ */
+    7781             : 
+    7782           2 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7783             : 
+    7784           2 :   if (!is_initialized_) {
+    7785           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7786             :   }
+    7787             : 
+    7788             :   {
+    7789           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    7790             : 
+    7791           2 :     std_srvs::TriggerResponse::ConstPtr response;
+    7792           2 :     std_srvs::TriggerRequest            request;
+    7793             : 
+    7794             :     response =
+    7795           2 :         tracker_list_.at(active_tracker_idx_)->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7796             : 
+    7797           2 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7798             : 
+    7799           4 :       return std::tuple(response->success, response->message);
+    7800             : 
+    7801             :     } else {
+    7802             : 
+    7803           0 :       std::stringstream ss;
+    7804           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7805             : 
+    7806           0 :       return std::tuple(false, ss.str());
+    7807             :     }
+    7808             :   }
+    7809             : }
+    7810             : 
+    7811             : //}
+    7812             : 
+    7813             : // | ----------------- service client wrappers ---------------- |
+    7814             : 
+    7815             : /* arming() //{ */
+    7816             : 
+    7817          18 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7818             : 
+    7819          36 :   std::stringstream ss;
+    7820             : 
+    7821          18 :   if (input) {
+    7822             : 
+    7823           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7824           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7825           0 :     return std::tuple(false, ss.str());
+    7826             :   }
+    7827             : 
+    7828          18 :   if (!input && !isOffboard()) {
+    7829             : 
+    7830           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7831           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7832           0 :     return std::tuple(false, ss.str());
+    7833             :   }
+    7834             : 
+    7835          18 :   if (!input && _rc_emergency_handoff_) {
+    7836             : 
+    7837           0 :     toggleOutput(false);
+    7838             : 
+    7839           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7840             :   }
+    7841             : 
+    7842          18 :   std_srvs::SetBool srv_out;
+    7843             : 
+    7844          18 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7845             : 
+    7846          18 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7847             : 
+    7848          18 :   if (sch_arming_.call(srv_out)) {
+    7849             : 
+    7850          18 :     if (srv_out.response.success) {
+    7851             : 
+    7852          18 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7853          18 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7854             : 
+    7855          18 :       if (!input) {
+    7856             : 
+    7857          18 :         toggleOutput(false);
+    7858             : 
+    7859          18 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7860          18 :         timer_failsafe_.stop();
+    7861          18 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7862             : 
+    7863          18 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7864          18 :         timer_eland_.stop();
+    7865          18 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7866             :       }
+    7867             : 
+    7868             :     } else {
+    7869           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7870           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7871             :     }
+    7872             : 
+    7873             :   } else {
+    7874           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7875           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7876             :   }
+    7877             : 
+    7878          36 :   return std::tuple(srv_out.response.success, ss.str());
+    7879             : }
+    7880             : 
+    7881             : //}
+    7882             : 
+    7883             : /* odometryCallbacksSrv() //{ */
+    7884             : 
+    7885          16 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7886             : 
+    7887          16 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7888             : 
+    7889          32 :   std_srvs::SetBool srv;
+    7890             : 
+    7891          16 :   srv.request.data = input;
+    7892             : 
+    7893          16 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7894             : 
+    7895          16 :   if (res) {
+    7896             : 
+    7897          16 :     if (!srv.response.success) {
+    7898           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7899             :     }
+    7900             : 
+    7901             :   } else {
+    7902           0 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7903             :   }
+    7904          16 : }
+    7905             : 
+    7906             : //}
+    7907             : 
+    7908             : /* elandSrv() //{ */
+    7909             : 
+    7910           8 : bool ControlManager::elandSrv(void) {
+    7911             : 
+    7912           8 :   ROS_INFO("[ControlManager]: calling for eland");
+    7913             : 
+    7914          16 :   std_srvs::Trigger srv;
+    7915             : 
+    7916           8 :   bool res = sch_eland_.call(srv);
+    7917             : 
+    7918           8 :   if (res) {
+    7919             : 
+    7920           8 :     if (!srv.response.success) {
+    7921           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    7922             :     }
+    7923             : 
+    7924           8 :     return srv.response.success;
+    7925             : 
+    7926             :   } else {
+    7927             : 
+    7928           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    7929             : 
+    7930           0 :     return false;
+    7931             :   }
+    7932             : }
+    7933             : 
+    7934             : //}
+    7935             : 
+    7936             : /* parachuteSrv() //{ */
+    7937             : 
+    7938           0 : bool ControlManager::parachuteSrv(void) {
+    7939             : 
+    7940           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    7941             : 
+    7942           0 :   std_srvs::Trigger srv;
+    7943             : 
+    7944           0 :   bool res = sch_parachute_.call(srv);
+    7945             : 
+    7946           0 :   if (res) {
+    7947             : 
+    7948           0 :     if (!srv.response.success) {
+    7949           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    7950             :     }
+    7951             : 
+    7952           0 :     return srv.response.success;
+    7953             : 
+    7954             :   } else {
+    7955             : 
+    7956           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    7957             : 
+    7958           0 :     return false;
+    7959             :   }
+    7960             : }
+    7961             : 
+    7962             : //}
+    7963             : 
+    7964             : /* ungripSrv() //{ */
+    7965             : 
+    7966         118 : void ControlManager::ungripSrv(void) {
+    7967             : 
+    7968         236 :   std_srvs::Trigger srv;
+    7969             : 
+    7970         118 :   bool res = sch_ungrip_.call(srv);
+    7971             : 
+    7972         118 :   if (res) {
+    7973             : 
+    7974           0 :     if (!srv.response.success) {
+    7975           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    7976             :     }
+    7977             : 
+    7978             :   } else {
+    7979         118 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    7980             :   }
+    7981         118 : }
+    7982             : 
+    7983             : //}
+    7984             : 
+    7985             : // | ------------------------ routines ------------------------ |
+    7986             : 
+    7987             : /* toggleOutput() //{ */
+    7988             : 
+    7989         129 : void ControlManager::toggleOutput(const bool& input) {
+    7990             : 
+    7991         129 :   if (input == output_enabled_) {
+    7992           6 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    7993           6 :     return;
+    7994             :   }
+    7995             : 
+    7996         123 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    7997             : 
+    7998         123 :   output_enabled_ = input;
+    7999             : 
+    8000             :   // if switching output off, switch to NullTracker
+    8001         123 :   if (!output_enabled_) {
+    8002             : 
+    8003          20 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    8004             : 
+    8005          20 :     switchTracker(_null_tracker_name_);
+    8006             : 
+    8007          20 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    8008             : 
+    8009          20 :     switchController(_eland_controller_name_);
+    8010             : 
+    8011             :     // | --------- deactivate all trackers and controllers -------- |
+    8012             : 
+    8013         140 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    8014             : 
+    8015         120 :       std::map<std::string, TrackerParams>::iterator it;
+    8016         120 :       it = trackers_.find(_tracker_names_.at(i));
+    8017             : 
+    8018             :       try {
+    8019         120 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    8020         120 :         tracker_list_.at(i)->deactivate();
+    8021             :       }
+    8022           0 :       catch (std::runtime_error& ex) {
+    8023           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    8024             :       }
+    8025             :     }
+    8026             : 
+    8027         120 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    8028             : 
+    8029         100 :       std::map<std::string, ControllerParams>::iterator it;
+    8030         100 :       it = controllers_.find(_controller_names_.at(i));
+    8031             : 
+    8032             :       try {
+    8033         100 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    8034         100 :         controller_list_.at(i)->deactivate();
+    8035             :       }
+    8036           0 :       catch (std::runtime_error& ex) {
+    8037           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    8038             :       }
+    8039             :     }
+    8040             : 
+    8041          20 :     timer_failsafe_.stop();
+    8042          20 :     timer_eland_.stop();
+    8043          20 :     timer_pirouette_.stop();
+    8044             : 
+    8045          20 :     offboard_mode_was_true_ = false;
+    8046             :   }
+    8047             : }
+    8048             : 
+    8049             : //}
+    8050             : 
+    8051             : /* switchTracker() //{ */
+    8052             : 
+    8053         242 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    8054             : 
+    8055         726 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    8056         726 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    8057             : 
+    8058             :   // copy member variables
+    8059         484 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8060         242 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8061             : 
+    8062         484 :   std::stringstream ss;
+    8063             : 
+    8064         242 :   if (!got_uav_state_) {
+    8065             : 
+    8066           0 :     ss << "can not switch tracker, missing odometry!";
+    8067           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8068           0 :     return std::tuple(false, ss.str());
+    8069             :   }
+    8070             : 
+    8071         242 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8072             : 
+    8073           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    8074           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8075           0 :     return std::tuple(false, ss.str());
+    8076             :   }
+    8077             : 
+    8078         242 :   if (rc_goto_active_) {
+    8079           0 :     ss << "can not switch tracker, the RC joystick is active";
+    8080           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    8081           0 :     return std::tuple(false, ss.str());
+    8082             :   }
+    8083             : 
+    8084         242 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    8085             : 
+    8086             :   // check if the tracker exists
+    8087         242 :   if (!new_tracker_idx) {
+    8088           2 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    8089           2 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8090           2 :     return std::tuple(false, ss.str());
+    8091             :   }
+    8092             : 
+    8093             :   // check if the tracker is already active
+    8094         240 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    8095          11 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    8096          11 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8097          11 :     return std::tuple(true, ss.str());
+    8098             :   }
+    8099             : 
+    8100             :   {
+    8101         229 :     std::scoped_lock lock(mutex_tracker_list_);
+    8102             : 
+    8103             :     try {
+    8104             : 
+    8105         229 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_.at(new_tracker_idx.value()).c_str());
+    8106             : 
+    8107         229 :       auto [success, message] = tracker_list_.at(new_tracker_idx.value())->activate(last_tracker_cmd);
+    8108             : 
+    8109         229 :       if (!success) {
+    8110             : 
+    8111           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    8112           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8113           0 :         return std::tuple(false, ss.str());
+    8114             : 
+    8115             :       } else {
+    8116             : 
+    8117         229 :         ss << "the tracker '" << tracker_name << "' was activated";
+    8118         229 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8119             : 
+    8120             :         {
+    8121         458 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8122             : 
+    8123             :           // update the time (used in failsafe)
+    8124         229 :           controller_tracker_switch_time_ = ros::Time::now();
+    8125             :         }
+    8126             : 
+    8127             :         // super important, switch the active tracker idx
+    8128             :         try {
+    8129             : 
+    8130         229 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+    8131         229 :           tracker_list_.at(active_tracker_idx_)->deactivate();
+    8132             : 
+    8133             :           // if switching from null tracker, re-activate the already active the controller
+    8134         229 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    8135             : 
+    8136         100 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+    8137             :             {
+    8138         200 :               std::scoped_lock lock(mutex_controller_list_);
+    8139             : 
+    8140         100 :               initializeControlOutput();
+    8141             : 
+    8142         200 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8143             : 
+    8144         100 :               controller_list_.at(active_controller_idx_)->activate(last_control_output);
+    8145             : 
+    8146             :               {
+    8147         200 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8148             : 
+    8149             :                 // update the time (used in failsafe)
+    8150         100 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8151             :               }
+    8152             :             }
+    8153             : 
+    8154             :             // if switching to null tracker, deactivate the active controller
+    8155         129 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8156             : 
+    8157          17 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+    8158             :             {
+    8159          34 :               std::scoped_lock lock(mutex_controller_list_);
+    8160             : 
+    8161          17 :               controller_list_.at(active_controller_idx_)->deactivate();
+    8162             :             }
+    8163             : 
+    8164             :             {
+    8165          17 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8166             : 
+    8167          17 :               last_tracker_cmd_ = {};
+    8168             :             }
+    8169             : 
+    8170          17 :             initializeControlOutput();
+    8171             :           }
+    8172             : 
+    8173         229 :           active_tracker_idx_ = new_tracker_idx.value();
+    8174             :         }
+    8175           0 :         catch (std::runtime_error& exrun) {
+    8176           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+    8177             :         }
+    8178             :       }
+    8179             :     }
+    8180           0 :     catch (std::runtime_error& exrun) {
+    8181           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8182           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8183             :     }
+    8184             :   }
+    8185             : 
+    8186         229 :   return std::tuple(true, ss.str());
+    8187             : }
+    8188             : 
+    8189             : //}
+    8190             : 
+    8191             : /* switchController() //{ */
+    8192             : 
+    8193         245 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8194             : 
+    8195         735 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8196         735 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8197             : 
+    8198             :   // copy member variables
+    8199         490 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8200         245 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8201             : 
+    8202         490 :   std::stringstream ss;
+    8203             : 
+    8204         245 :   if (!got_uav_state_) {
+    8205             : 
+    8206           0 :     ss << "can not switch controller, missing odometry!";
+    8207           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8208           0 :     return std::tuple(false, ss.str());
+    8209             :   }
+    8210             : 
+    8211         245 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8212             : 
+    8213           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8214           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8215           0 :     return std::tuple(false, ss.str());
+    8216             :   }
+    8217             : 
+    8218         245 :   if (rc_goto_active_) {
+    8219           2 :     ss << "can not switch controller, the RC joystick is active";
+    8220           2 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    8221           2 :     return std::tuple(false, ss.str());
+    8222             :   }
+    8223             : 
+    8224         243 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8225             : 
+    8226             :   // check if the controller exists
+    8227         243 :   if (!new_controller_idx) {
+    8228           2 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8229           2 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8230           2 :     return std::tuple(false, ss.str());
+    8231             :   }
+    8232             : 
+    8233             :   // check if the controller is not active
+    8234         241 :   if (new_controller_idx.value() == active_controller_idx) {
+    8235             : 
+    8236          34 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8237          34 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8238          34 :     return std::tuple(true, ss.str());
+    8239             :   }
+    8240             : 
+    8241             :   {
+    8242         207 :     std::scoped_lock lock(mutex_controller_list_);
+    8243             : 
+    8244             :     try {
+    8245             : 
+    8246         207 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_.at(new_controller_idx.value()).c_str());
+    8247         207 :       if (!controller_list_.at(new_controller_idx.value())->activate(last_control_output)) {
+    8248             : 
+    8249           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8250           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8251           0 :         return std::tuple(false, ss.str());
+    8252             : 
+    8253             :       } else {
+    8254             : 
+    8255         207 :         ss << "the controller '" << controller_name << "' was activated";
+    8256         207 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8257             : 
+    8258         207 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_.at(new_controller_idx.value()).c_str(),
+    8259             :                  _tracker_names_.at(active_tracker_idx_).c_str());
+    8260             : 
+    8261             :         // reactivate the current tracker
+    8262             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8263             :         // but it serves the purpose
+    8264             :         {
+    8265         207 :           std::scoped_lock lock(mutex_tracker_list_);
+    8266             : 
+    8267         207 :           tracker_list_.at(active_tracker_idx_)->deactivate();
+    8268         207 :           tracker_list_.at(active_tracker_idx_)->activate({});
+    8269             :         }
+    8270             : 
+    8271             :         {
+    8272         414 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8273             : 
+    8274             :           // update the time (used in failsafe)
+    8275         207 :           controller_tracker_switch_time_ = ros::Time::now();
+    8276             :         }
+    8277             : 
+    8278             :         // super important, switch the active controller idx
+    8279             :         try {
+    8280             : 
+    8281         207 :           controller_list_.at(active_controller_idx_)->deactivate();
+    8282         207 :           active_controller_idx_ = new_controller_idx.value();
+    8283             :         }
+    8284           0 :         catch (std::runtime_error& exrun) {
+    8285           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+    8286             :         }
+    8287             :       }
+    8288             :     }
+    8289           0 :     catch (std::runtime_error& exrun) {
+    8290           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8291           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8292             :     }
+    8293             :   }
+    8294             : 
+    8295         207 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8296             : 
+    8297             :   {
+    8298         207 :     std::scoped_lock lock(mutex_constraints_);
+    8299             : 
+    8300         207 :     sanitized_constraints_ = current_constraints_;
+    8301         207 :     sanitized_constraints  = sanitized_constraints_;
+    8302             :   }
+    8303             : 
+    8304         207 :   setConstraintsToControllers(sanitized_constraints);
+    8305             : 
+    8306         207 :   return std::tuple(true, ss.str());
+    8307             : }
+    8308             : 
+    8309             : //}
+    8310             : 
+    8311             : /* updateTrackers() //{ */
+    8312             : 
+    8313      136027 : void ControlManager::updateTrackers(void) {
+    8314             : 
+    8315      272054 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8316      272054 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8317             : 
+    8318             :   // copy member variables
+    8319      136027 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8320      136027 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8321             : 
+    8322             :   // --------------------------------------------------------------
+    8323             :   // |                     Update the trackers                    |
+    8324             :   // --------------------------------------------------------------
+    8325             : 
+    8326           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8327             : 
+    8328             :   unsigned int active_tracker_idx;
+    8329             : 
+    8330             :   {
+    8331      136027 :     std::scoped_lock lock(mutex_tracker_list_);
+    8332             : 
+    8333      136027 :     active_tracker_idx = active_tracker_idx_;
+    8334             : 
+    8335             :     // for each tracker
+    8336      953892 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8337             : 
+    8338      817865 :       if (i == active_tracker_idx) {
+    8339             : 
+    8340             :         try {
+    8341             :           // active tracker => update and retrieve the command
+    8342      136027 :           tracker_command = tracker_list_.at(i)->update(uav_state, last_control_output);
+    8343             :         }
+    8344           0 :         catch (std::runtime_error& exrun) {
+    8345           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the active tracker (%s)",
+    8346             :                              _tracker_names_.at(active_tracker_idx).c_str());
+    8347           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8348           0 :           tracker_command = {};
+    8349             :         }
+    8350             : 
+    8351             :       } else {
+    8352             : 
+    8353             :         try {
+    8354             :           // nonactive tracker => just update without retrieving the command
+    8355      681838 :           tracker_list_.at(i)->update(uav_state, last_control_output);
+    8356             :         }
+    8357           0 :         catch (std::runtime_error& exrun) {
+    8358           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the tracker '%s'", _tracker_names_.at(i).c_str());
+    8359           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8360             :         }
+    8361             :       }
+    8362             :     }
+    8363             : 
+    8364      136027 :     if (active_tracker_idx == _null_tracker_idx_) {
+    8365       36618 :       return;
+    8366             :     }
+    8367             :   }
+    8368             : 
+    8369       99409 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8370             : 
+    8371      198818 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8372             : 
+    8373       99409 :     last_tracker_cmd_ = tracker_command;
+    8374             : 
+    8375             :     // | --------- fill in the potentially missing header --------- |
+    8376             : 
+    8377       99409 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8378           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8379             :     }
+    8380             : 
+    8381       99409 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8382           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8383             :     }
+    8384             : 
+    8385             :   } else {
+    8386             : 
+    8387           0 :     if (active_tracker_idx == _ehover_tracker_idx_) {
+    8388             : 
+    8389           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the emergency tracker '%s' returned empty or invalid command!",
+    8390             :                          _tracker_names_.at(active_tracker_idx).c_str());
+    8391           0 :       failsafe();
+    8392             : 
+    8393             :     } else {
+    8394             : 
+    8395           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_.at(active_tracker_idx).c_str());
+    8396             : 
+    8397           0 :       if (_tracker_error_action_ == ELAND_STR) {
+    8398           0 :         eland();
+    8399           0 :       } else if (_tracker_error_action_ == EHOVER_STR) {
+    8400           0 :         ehover();
+    8401             :       } else {
+    8402           0 :         failsafe();
+    8403             :       }
+    8404             :     }
+    8405             :   }
+    8406             : }
+    8407             : 
+    8408             : //}
+    8409             : 
+    8410             : /* updateControllers() //{ */
+    8411             : 
+    8412      146218 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8413             : 
+    8414      292436 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8415      292436 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8416             : 
+    8417             :   // copy member variables
+    8418      146218 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8419             : 
+    8420             :   // | ----------------- update the controllers ----------------- |
+    8421             : 
+    8422             :   // the trackers are not running
+    8423      146218 :   if (!last_tracker_cmd) {
+    8424             : 
+    8425             :     {
+    8426       73236 :       std::scoped_lock lock(mutex_controller_list_);
+    8427             : 
+    8428             :       // nonactive controller => just update without retrieving the command
+    8429      219708 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8430      183090 :         controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+    8431             :       }
+    8432             :     }
+    8433             : 
+    8434       36618 :     return;
+    8435             :   }
+    8436             : 
+    8437      219200 :   Controller::ControlOutput control_output;
+    8438             : 
+    8439             :   unsigned int active_controller_idx;
+    8440             : 
+    8441             :   {
+    8442      219200 :     std::scoped_lock lock(mutex_controller_list_);
+    8443             : 
+    8444      109600 :     active_controller_idx = active_controller_idx_;
+    8445             : 
+    8446             :     // for each controller
+    8447      657600 :     for (size_t i = 0; i < controller_list_.size(); i++) {
+    8448             : 
+    8449      548000 :       if (i == active_controller_idx) {
+    8450             : 
+    8451             :         try {
+    8452             :           // active controller => update and retrieve the command
+    8453      109600 :           control_output = controller_list_.at(active_controller_idx)->updateActive(uav_state, last_tracker_cmd.value());
+    8454             :         }
+    8455           0 :         catch (std::runtime_error& exrun) {
+    8456             : 
+    8457           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: an exception while updating the active controller (%s)",
+    8458             :                              _controller_names_.at(active_controller_idx).c_str());
+    8459           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8460             :         }
+    8461             : 
+    8462             :       } else {
+    8463             : 
+    8464             :         try {
+    8465             :           // nonactive controller => just update without retrieving the command
+    8466      438400 :           controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+    8467             :         }
+    8468           0 :         catch (std::runtime_error& exrun) {
+    8469             : 
+    8470           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_.at(i).c_str());
+    8471           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8472             :         }
+    8473             :       }
+    8474             :     }
+    8475             :   }
+    8476             : 
+    8477             :   // normally, the active controller returns a valid command
+    8478      109600 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8479             : 
+    8480      109600 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8481             : 
+    8482             :     // but it can return an empty command, due to some critical internal error
+    8483             :     // which means we should trigger the failsafe landing
+    8484             :   } else {
+    8485             : 
+    8486             :     // only if the controller is still active, trigger escalating failsafe
+    8487             :     // if not active, we don't care, we should not ask the controller for
+    8488             :     // the result anyway -> this could mean a race condition occured
+    8489             :     // like it once happend during landing
+    8490           0 :     bool controller_status = false;
+    8491             : 
+    8492             :     {
+    8493           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8494             : 
+    8495           0 :       controller_status = controller_list_.at(active_controller_idx)->getStatus().active;
+    8496             :     }
+    8497             : 
+    8498           0 :     if (controller_status) {
+    8499             : 
+    8500           0 :       if (failsafe_triggered_) {
+    8501             : 
+    8502           0 :         ROS_ERROR("[ControlManager]: disabling control, the active controller returned an empty command when failsafe was active");
+    8503             : 
+    8504           0 :         toggleOutput(false);
+    8505             : 
+    8506           0 :       } else if (eland_triggered_) {
+    8507             : 
+    8508           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the active controller returned an empty command when eland was active");
+    8509             : 
+    8510           0 :         failsafe();
+    8511             : 
+    8512             :       } else {
+    8513             : 
+    8514           0 :         ROS_ERROR("[ControlManager]: triggering eland, the active controller returned an empty command");
+    8515             : 
+    8516           0 :         eland();
+    8517             :       }
+    8518             :     }
+    8519             :   }
+    8520             : }
+    8521             : 
+    8522             : //}
+    8523             : 
+    8524             : /* publish() //{ */
+    8525             : 
+    8526      146218 : void ControlManager::publish(void) {
+    8527             : 
+    8528      292436 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8529      292436 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8530             : 
+    8531             :   // copy member variables
+    8532      146218 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8533      146218 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8534      146218 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8535      146218 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8536      146218 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8537             : 
+    8538      146218 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8539             : 
+    8540             :   // --------------------------------------------------------------
+    8541             :   // |                 Publish the control command                |
+    8542             :   // --------------------------------------------------------------
+    8543             : 
+    8544      146218 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8545      146218 :   attitude_target.stamp = ros::Time::now();
+    8546             : 
+    8547      146218 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8548      146218 :   attitude_rate_target.stamp = ros::Time::now();
+    8549             : 
+    8550      146218 :   if (!output_enabled_) {
+    8551             : 
+    8552       22506 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8553             : 
+    8554      123712 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8555             : 
+    8556       14116 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8557             : 
+    8558             :     Controller::HwApiOutputVariant output =
+    8559       28232 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8560             : 
+    8561             :     {
+    8562       28232 :       std::scoped_lock lock(mutex_last_control_output_);
+    8563             : 
+    8564       14116 :       last_control_output_.control_output = output;
+    8565             :     }
+    8566             : 
+    8567       14116 :     control_output_publisher_.publish(output);
+    8568             : 
+    8569      109596 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8570             : 
+    8571           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8572             :                       _controller_names_.at(active_controller_idx).c_str());
+    8573             : 
+    8574             :     Controller::HwApiOutputVariant output =
+    8575           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8576             : 
+    8577           0 :     control_output_publisher_.publish(output);
+    8578             : 
+    8579      109596 :   } else if (last_control_output.control_output) {
+    8580             : 
+    8581      109596 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8582      109596 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8583             :     } else {
+    8584           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8585           0 :       return;
+    8586             :     }
+    8587             : 
+    8588             :   } else {
+    8589           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8590             :   }
+    8591             : 
+    8592             :   // | ----------- publish the controller diagnostics ----------- |
+    8593             : 
+    8594      146218 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8595             : 
+    8596             :   // | --------- publish the applied throttle and thrust -------- |
+    8597             : 
+    8598      146218 :   auto throttle = extractThrottle(last_control_output);
+    8599             : 
+    8600      146218 :   if (throttle) {
+    8601             : 
+    8602             :     {
+    8603      117966 :       std_msgs::Float64 msg;
+    8604      117966 :       msg.data = throttle.value();
+    8605      117966 :       ph_throttle_.publish(msg);
+    8606             :     }
+    8607             : 
+    8608      117966 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8609             : 
+    8610             :     {
+    8611      117966 :       std_msgs::Float64 msg;
+    8612      117966 :       msg.data = thrust;
+    8613      117966 :       ph_thrust_.publish(msg);
+    8614             :     }
+    8615             :   }
+    8616             : 
+    8617             :   // | ----------------- publish tracker command ---------------- |
+    8618             : 
+    8619      146218 :   if (last_tracker_cmd) {
+    8620      109596 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8621             :   }
+    8622             : 
+    8623             :   // | --------------- publish the odometry input --------------- |
+    8624             : 
+    8625      146218 :   if (last_control_output.control_output) {
+    8626             : 
+    8627      249432 :     mrs_msgs::EstimatorInput msg;
+    8628             : 
+    8629      124716 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8630      124716 :     msg.header.stamp    = ros::Time::now();
+    8631             : 
+    8632      124716 :     if (last_control_output.desired_unbiased_acceleration) {
+    8633       94724 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()(0);
+    8634       94724 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()(1);
+    8635       94724 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()(2);
+    8636             :     }
+    8637             : 
+    8638      124716 :     if (last_control_output.desired_heading_rate) {
+    8639       91368 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8640             :     }
+    8641             : 
+    8642      124716 :     if (last_control_output.desired_unbiased_acceleration) {
+    8643       94724 :       ph_mrs_odom_input_.publish(msg);
+    8644             :     }
+    8645             :   }
+    8646             : }
+    8647             : 
+    8648             : //}
+    8649             : 
+    8650             : /* deployParachute() //{ */
+    8651             : 
+    8652           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8653             : 
+    8654             :   // if not enabled, return false
+    8655           0 :   if (!_parachute_enabled_) {
+    8656             : 
+    8657           0 :     std::stringstream ss;
+    8658           0 :     ss << "can not deploy parachute, it is disabled";
+    8659           0 :     return std::tuple(false, ss.str());
+    8660             :   }
+    8661             : 
+    8662             :   // we can not disarm if the drone is not in offboard mode
+    8663             :   // this is super important!
+    8664           0 :   if (!isOffboard()) {
+    8665             : 
+    8666           0 :     std::stringstream ss;
+    8667           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8668           0 :     return std::tuple(false, ss.str());
+    8669             :   }
+    8670             : 
+    8671             :   // call the parachute service
+    8672           0 :   bool succ = parachuteSrv();
+    8673             : 
+    8674             :   // if the deployment was successful,
+    8675           0 :   if (succ) {
+    8676             : 
+    8677           0 :     arming(false);
+    8678             : 
+    8679           0 :     std::stringstream ss;
+    8680           0 :     ss << "parachute deployed";
+    8681             : 
+    8682           0 :     return std::tuple(true, ss.str());
+    8683             : 
+    8684             :   } else {
+    8685             : 
+    8686           0 :     std::stringstream ss;
+    8687           0 :     ss << "error during deployment of parachute";
+    8688             : 
+    8689           0 :     return std::tuple(false, ss.str());
+    8690             :   }
+    8691             : }
+    8692             : 
+    8693             : //}
+    8694             : 
+    8695             : /* velocityReferenceToReference() //{ */
+    8696             : 
+    8697         737 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8698             : 
+    8699        1474 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8700        1474 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8701         737 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8702             : 
+    8703         737 :   mrs_msgs::ReferenceStamped reference_out;
+    8704             : 
+    8705         737 :   reference_out.header = vel_reference.header;
+    8706             : 
+    8707         737 :   if (vel_reference.reference.use_heading) {
+    8708           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8709         737 :   } else if (vel_reference.reference.use_heading_rate) {
+    8710         643 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8711             :   } else {
+    8712          94 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8713             :   }
+    8714             : 
+    8715         737 :   if (vel_reference.reference.use_altitude) {
+    8716           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8717             :   } else {
+    8718             : 
+    8719         737 :     double stopping_time_z = 0;
+    8720             : 
+    8721         737 :     if (vel_reference.reference.velocity.x >= 0) {
+    8722         515 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8723             :     } else {
+    8724         222 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8725             :     }
+    8726             : 
+    8727         737 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8728             :   }
+    8729             : 
+    8730             :   {
+    8731         737 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8732         737 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8733             : 
+    8734         737 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8735         737 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8736             :   }
+    8737             : 
+    8738        1474 :   return reference_out;
+    8739             : }
+    8740             : 
+    8741             : //}
+    8742             : 
+    8743             : /* publishControlReferenceOdom() //{ */
+    8744             : 
+    8745      146218 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8746             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8747             : 
+    8748      146218 :   if (!tracker_command || !control_output.control_output) {
+    8749       36622 :     return;
+    8750             :   }
+    8751             : 
+    8752      219192 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8753             : 
+    8754      219192 :   nav_msgs::Odometry msg;
+    8755             : 
+    8756      109596 :   msg.header = tracker_command->header;
+    8757             : 
+    8758      109596 :   if (tracker_command->use_position_horizontal) {
+    8759      109596 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8760      109596 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8761             :   } else {
+    8762           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8763           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8764             :   }
+    8765             : 
+    8766      109596 :   if (tracker_command->use_position_vertical) {
+    8767      109596 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8768             :   } else {
+    8769           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8770             :   }
+    8771             : 
+    8772             :   // transform the velocity in the reference to the child_frame
+    8773      109596 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8774             : 
+    8775      109596 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8776             : 
+    8777      219192 :     geometry_msgs::Vector3Stamped velocity;
+    8778      109596 :     velocity.header = tracker_command->header;
+    8779             : 
+    8780      109596 :     if (tracker_command->use_velocity_horizontal) {
+    8781      109596 :       velocity.vector.x = tracker_command->velocity.x;
+    8782      109596 :       velocity.vector.y = tracker_command->velocity.y;
+    8783             :     }
+    8784             : 
+    8785      109596 :     if (tracker_command->use_velocity_vertical) {
+    8786      109596 :       velocity.vector.z = tracker_command->velocity.z;
+    8787             :     }
+    8788             : 
+    8789      219192 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8790             : 
+    8791      109596 :     if (res) {
+    8792      109596 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8793      109596 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8794      109596 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8795             :     } else {
+    8796           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8797             :                          msg.child_frame_id.c_str());
+    8798             :     }
+    8799             :   }
+    8800             : 
+    8801             :   // fill in the orientation or heading
+    8802      109596 :   if (control_output.desired_orientation) {
+    8803       92662 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8804       16934 :   } else if (tracker_command->use_heading) {
+    8805       16934 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8806             :   }
+    8807             : 
+    8808             :   // fill in the attitude rate
+    8809      109596 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8810             : 
+    8811       86467 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8812             : 
+    8813       86467 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8814       86467 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8815       86467 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8816             :   }
+    8817             : 
+    8818      109596 :   ph_control_reference_odom_.publish(msg);
+    8819             : }
+    8820             : 
+    8821             : //}
+    8822             : 
+    8823             : /* initializeControlOutput() //{ */
+    8824             : 
+    8825         224 : void ControlManager::initializeControlOutput(void) {
+    8826             : 
+    8827         224 :   Controller::ControlOutput controller_output;
+    8828             : 
+    8829         224 :   controller_output.diagnostics.total_mass      = _uav_mass_;
+    8830         224 :   controller_output.diagnostics.mass_difference = 0.0;
+    8831             : 
+    8832         224 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8833         224 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8834             : 
+    8835         224 :   if (std::abs(_initial_body_disturbance_x_) > 0.001 || std::abs(_initial_body_disturbance_y_) > 0.001) {
+    8836           0 :     controller_output.diagnostics.disturbance_estimator = true;
+    8837             :   }
+    8838             : 
+    8839         224 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8840         224 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8841             : 
+    8842         224 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8843         224 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8844             : 
+    8845         224 :   controller_output.diagnostics.controller = "none";
+    8846             : 
+    8847         224 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8848         224 : }
+    8849             : 
+    8850             : //}
+    8851             : 
+    8852             : }  // namespace control_manager
+    8853             : 
+    8854             : }  // namespace mrs_uav_managers
+    8855             : 
+    8856             : #include <pluginlib/class_list_macros.h>
+    8857         107 : 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..b4f80b7f24 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2235 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ 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..3e0c896ea605b79c8de7390791cdcb06b9118c62 GIT binary patch literal 25927 zcmV)VK(D`vP)oImN27<8*7l%>b^?=S<>EX~~zyb#;M%^k@kI_Pm z8qgq*YC!#R9X>{(_x7%3a+Ix^Jq8^-g$-? zN8rL3#|;x<95T>(e5}qjD`q2Aij1p$j1o?cn4va>jFIIdN09{BK29}82{0RH39xOP zEJmF%EHF|8Y-S{b(Ljt|hxce*4&w*Nqv3?au}1*xSlS4qg&4iv(>}OQ8soT)I$%8R zgFFtk0G89TEE?z4FVIr&c^2TQ4jTZMn*2wMHFGKgdf^e}DE;HHy!rr^GC2V7Sb_uW z86|LY1b3+19vsof`e{lHXzY3+C`G`LeNUJfDApyw;XR|Si`XZ0eRLA(*UcD=JH9&L zPP78!c&ZO-`BTV)t*J!J)ZFe7G~_V}Q@f4(F;l1p)No_oTn}?qe9p`~IzLwSxwsUR zfQuUAX&9ikLF;IccjgG}@F5qqV1p}LQU;7#QW1On-SY|kdBltF2#n0?>kj^F>7g_m#B+<|~x|KE}4@Yc~RZ#41Dwg%Q?(NpP2J zm+xtMOi{FJzkt^Cu3C!wHT+P*7y(DjC>mfb-b7_YVUpDt#~8V3Q}F)*ecGbhPqh|8 z6ybL)rW{bv>%r4m>UGI5mWWV=Q6j1XA2Ay{w#X_U!zNVaV_2yjER>>U5E3-}&P zr29KyjE>LrktaqMNVb1ZoZN7-8>rTXd?;@LEnWMi02z5qoak5z2`~GN3JY8fN*C8YWURDRaF z0-}(z_Zuhl*)|Xpw9*1ub4-`9VVn&|L`B!BBdxW!Lmn4kGaPr>JDzgD)8QEWbnZ8t zHedjkqa*g{Bu0U|3K`zjO+1dVq!0+VBhV6O#u^v><$eBE=oW^Mg?O6z$$p`u#du$6L*3$e#a0fRj` ztgIg_(vTR~>wiRkT&P3ai7B+Si##AZF*XB=GeZDNQ@EJ; z_&PxQ%q#_}A)swJaDKDS{TgB;BH#W!gN=7p&26_r`gS2yHlu*5s7GNggl@cKF>EuYzCz!#QZ}7;A!qyC#jeBWr+ai~wZ? z@D$-|fNkt?SCUPOt8c%K3p*ajp&z8l-xGb=$C6kwEab!eIIhCqG7pg(W5r5AgOe-( zma=vw6&)I3VxS{npbsGT*XS|AWmE{0!VB@{TNYP%IOs6zMYTW z>WR~FlucK}^+on*9*C?7E+is{og(Ilfv1U6@?Z~OPwssu#L%lS31i&?7XbUEhca9S zJkW*Teec@J#{w}&FuH??(Kbdm!eh905ssuSbA3GS;V}IH1E&!2Qy5}4M%AtTkRjqr zl9Jqj(Zy+00gA45ATbINQW1k&U{z()L?@FT%ig&l(q8HB&oaOiE*6;?hl|FTd!ziM zqevSFm`Gv)$x?7)#Cni&Q$1izW4Evet^XnE9O$M3-njT)>=@DdA2CQQh$tkG0m0$>B%NU;x-4^xhrJU^j;&M$j;R@Jcm*3rwA%< z37Z|bvWHWi>7M+jUO*1^D5yOFFnkXeY?0v=Km)^vYqMjvqX>XVFJx@$1zdM>{;33bf=A9WAVAtiOa+jhq`0A; z1gaTeftVap06b#QMP|=J<+;E|jCBAY8Rn?D-rEw?UJX!cNk#IOWIlkAAw;4vBIu?+FmH`=U#(m%8P-O~b%e+SK)|;8;yQvMMjrGyD#I9$C0w{i^%zGCGO#f6GL4q$ zIu5ol&|J&}J;edG5dnz-gd9bLkb7l}km1QE)-h5%#w8q1g$RxKq0nJAGtxL2hCe1H zpB!#xq*=66T|B|UnW=LgFogG4~Y$&Rt1u9=n{XHP;qVlY$Jjcy$o<q{;w19v!C#L^sf6y^pJ%#$NVR_74 zlQtjsjte;Hv8TPN2dN=9<(Q(W-M!rsf23XKs~R~%t+SeYUH#8=DrskNU<@Gs4Imul zu1|jO8O9|8TzuCCO34^?N)G{Q>+RLwV^WRrNTxdOi=Lf!j8fYV!(akLz&I8U*hEY? z=R&bo z3r&;sPy{1fcB$gATkL!|QnubY?)3u35@VWb=oL}SNB`(p7uDzdVZ;0dK_f=kN`P$`{Tpy1U>x`+_D;l%eKnEk8gfbm z+&o6j!*V)6k5Q6%@(yG^n_C1_HiS_SkG-`{;Yy`A{`^VnddQo*uI1~R*O48oaKpkN z0RH~F4p576({~+3EeC9)I!JELNl{6F;DrcePaKkN$%9hMBi~1;_--04@ASP>XCSr&ye!*c5J;h(jzbg=vZHJ8U!K;qGZJ6WcAdFyoN`PR-P4cLpqLVK8 zL)r`dAc66rW>3Pn$p_Nu7C;Gk12J2oh--!^JhUvd@)VLeMBXt>wFrpqM?HHy#?o2A z0qQ1OPxa5w{eHXCcD>zxaQg-PAu!?3Yxw!`zdHo{DYaif+KC9bk1-n)11OC#jxqxr zG)9A%p;_H_`PtGOC!u(zwTC+I(&6m+zV(FUSH(uZ*|tMI)o8M>x5xi)yF99<`gg8% zQ*D{+BEQpGS6L~W!0;U1Wym&+RJ3eAX22ytgj3h`CXW$e>lR$v*76w17&I7nly$&O zhcUTl^|qM~VYTas^10R?Iw-Yt{ky#a%VYo8=##?x^ok_$v57mc38SRQ&)3J!cKd84 zMc|r*;xQM%_#so%J~JEsIZjP=W^OmPjlAv5Acm{pn)D-{7`kNIX9iD9S~As{2@~^% zYbJM<6lMk;UDX?Vb>|wbnhm=`1b%wg&m1GsRPj3##-fsPqx}8qBa~Fib{P~BTb;td zERD&vX#l~RQ3H;VaMyHZp&!C9ssZ3gVm4*h7ws7J>~Vl&h*4Y3q2qB%3IYzBnN`*G zMSc1J2cP~iV)fVOeqhwI`Hp?0W09vCn7SqQ=r6iiK8k=Cblg&~sAk>yIt-v&%(5_+ zHw4TkTm~ffIwBywhU^E_k1+$(PGKxP*=#Bd=OYaz(HZPp3)O6W4TFI++%aTW*Y;fj zTz>Y>HJQTb>JfuEwPPG|5Z5tW>G~ucPq`%>X_4-u7~S=zG&I+dmXvlWfK@o4YvMQ` z#>Z&wX))4AH+*0Rw_K5fXLG&5t5R3=p3Md>X@2* zJjEb?7tvWDM#^ij8|;g^44Af7$|K8Y@(VAHhX;4 z&BqM!)^D3_M+$?E!ffggGJ8>*564tk;kmMaw*Y?QF;<)f#&BgqOCG&kAO_KLFB2nR z97K#ERYJR5bJ1SON)1avySN06OB8Nf@&(=hGC=N3*mJ0e*=f}qkh0lx@+kJMmCLn( zJ)SS@!8ilNN4>3pu??`17*$ekUF}8R-ta7qvoa}Hp_=19GjWWyeAmht6W~lfmIkTd z?|GoK<9j8wF%lun^w3I|LTih`o(6^a|y5+Ad>Hk7hL z8)4czX8OZnW>$HwRZUh>%^`zrN~$1{-AXEx$d8ama$Qh74XfHTE2UQY3hW zA%9YWag~wlSU?cXUB##e+|(FX6<|y{Ed!cE)6ctce~|VMm6G0I=62hpbyyY9{M{2h>9-~09lk>1pK^Q05|MEp9`>GJv{y< zt57N7;*|!uZ!}+NfDxvjNIsWzgVU9!_00t!?ngCX6EWXhfU#rz<^uGM@m0A1hA|eB z20-ed2P`JqhO42cCk=QsZ=E#Yzxnu$ohW;);0_3X0+)+Vh%J;upvQzi!4T+=5Q_dO zB;i2Qr*K|)XuL7}tnkq9D*X6Wm{p(eFOMnCHJa+@M)=|kk&l>}K@q-9W$N1_e0eQ7 zW^q9+`59B#65$(T-QXGjS@rgUPcP(j;<;8zk06MtWq|Mil*c=f+EXw^MKPzS;@DVR$@R|$B;L8 z^glUq?2<<)6!{qC$>=A>t-wS_4>8E8ux203b=b)R&!~84Zyimx@}rfHcxK#}oIQ}_ z5&U)$5Q;$saCFydz_S(*qAUX{F`kuOpS6INF#=e{%sB8m;J~i2ILm-U?3Kju?tQ*^ zAPcB$NCAMP{bRcVXxIxwe2p+hH8B8VG2^A@#)ro`VRz+JGu%H0lsJA!4ofwnQjWbK zDup*ZFd&$m^otze<}r#>NV~2AdW`bj#{~{6cilJGW?29mf6k9$R=O~7(TX<^X&67S zckCX93t5hb&q+ysoOh%^b&d3CrYkqgc+_zzlmXN090kCTN#c4gW89LmuT^VZ8!V|Z zP6I}yq~hiMaiEfN9Tm%~2cmRsk#Mp%s9`1y_mDMvNMe&&uO##cKgCn-)-wvHTDN7-H8$4CDwPYEybb zssZwE0ThUl6}fecag+r>EqlPO5ch*4@GP_zlv6k)0ei%Cux5Oe7R;XIi@)Jle}+*H zD6IZWoB09p7;Sg+Xe@xH^p5F_H&= z@X8u)Xx6iO5)a-x6kNETu!+V1gzb_*I@z2R`CKoqZAt1N?@qmSLDVq^v?IxT|>G#yb^tf$M59q9#4##;cR3f<9RtK55tD8hdu(mx5k%m4%z#P zQ>)&>U;q7|83S$NtsYo|A!AWFg_kG?MO6Tgd-6HTqyV_(%I$R6jRV59_oRUk8+!>+ z@e1jkso#zrmU(*taxe#rZv4owSC(jH? zPUDE#G{o?$=9$E7#ifAW=Aub{ntAyufI2akd_Zj1$|m?o89=oiu5MH8aBt-v2jOOH z+<3Qn0ZY5d?kPq=yW^hXBsa5gY1t0nCfMQO(cgU@D`Q-fxU$3ZUBfw)ofVq`_eZZt8Yu4Z zbC>HW2JJuo4gk-1`J$YQ)F6*h_YK2{+Eb1IU4py7`LIu6Z*~D=+3Oh|oLV(NHAZ|` zkR&6|wFgSF2uSykjRoAl+G`DLcz5m~rXY{%atZIDaZkZgSU55KMBRf7uqKBu-Cc(U z7nh@FVb=vqvQ?2HW9qqpa;jE?lBy)p!sQCM$O6u&ZW8-O^kW&mwv zqWD_i$VZBzY+}u&5Ns0*5`1EXftQ%B!_-V@iDvYOAo6* zK8Ttk6w<83ruK=MPzT9E^=a3k`+eLJ?&W%JsRPD%#pi?LG0F!q`vubWAR<#3v5=FA zJv5nwuC9#%3<=H|8*_}TVZ2QP)Kk6kS{FV|V8gf%Qm#bUim^iaEI`c|AMLT)tKz*I z*)gYEw#iNJ_{=idL*1mfvD-YmTMNZmV7-Qu%801$ke8rd|3hkjAlXGe_--B|s5 zd@_5WN7jGdpTS7{NPzI$*a$|A3sNuMJ1u$3Hik)nnpTkTTr~?yd_^us0i)1$=NNH& z0IHnGRweaNQ}pKHe^F8+y0%#)zST4OP<>G=8>VPJU z`{1@xRrw~W0ePcxIoC@XqG%s@aL)d*w3E`Bkf zs$rohMCJ9}R0x2hQ+FmQiY117l>om++}DUJPT|*xYi;MhWW=f4`D^WsYBQ|8t?Z+? z_@@(*p*JK%>jLe^62|d>mNCA!P@KPp=GV}qfQB|&M+uh&3^8w&RE10Zd|Faft-zT% z4=MX?Zhz>w1qhfkK&?=`ef;X-Qz{`cxR`Zo!Kjq*Z*4_YZACXuMajgAyBrb#S5V+^ zPbAk?YkY3WL{OHenB|N%P4K-?QdXf>o>BHzSP)b{#uC21!u#sKTVF}cjJKuLdF+=c zx{;%dw3#-f5Xn)TAyTtIR#>mBS&;J)#Y;t!oW{MzRFXUS6x%_kZ{2e!?$>WOjDl2y z_y|5G314F11$>^o>zSic1*To?X$mW~Z0w;UuB_4-TL2s&h!~;+VjTvXws^- zo*>z1QKoS<9?lB>%7&eb3L^Qs$^Af=G@{=GXzse`Y?|Qt5crs_@i}}nV|*!l3g@^! zFPGyu{$L6nc(Ig@XVIz1PcfKl_xCv%X6S&nCbQRefDB$M;J$Ku85}u^YGl|R z1j@c&yG?Tm8*7XXY}&8U%aURD9&o6@Evu?y@$J6^82|tv07*naRM`WLCuRq_LD#ud zu)L3eX9>_;PDZ^J&Po%&e%oA?$aAt4gtql`ofog6RIeA&nUQA3n<>xgmj8J#sWE?R@fP`4)76K3#O>}GhKg@rZ8Y!OQ`lQ zS8Lt}=SJWF)fkBlutxD*6GH$-(0%@U7lTklWaVYM18P&rw4ym0PTx!yV)0SuPz@|@ z#ZON;VQS!YUp8%Y`uomY>@&h=-#4RbDd5IZnPJ3!v*@b3`Tu+s)!W_RH&f4yp~ktE zkGtAiH(@Vo#OOEn+I4D;j@0!ZH3L**6nD)n?w$}Mg2Qaz ztefVJOfyE!;(D}K?V7LMUAfIQoG0hLIG#`*V~y&BuzSB=v%`h8jNU`(kvk}Qo78$M zC84^xiUt;pKX8noz``{HRAVGJV%MnjZ~OmUS@eYQ?(XWqeI42a^ivqR){IfR&%AOK z9+QBdG7;~Q@S!gQ<*qok{F@phb040M_irCbh0V3&ND2^!KX!Yy31c1bRVPjLQpm-O zT@zn1t7BByaJ=W^LKwCOa~h`btg+$#p64UkJxj^%@fFUGDdfXnLbxCRUnt#>3Uj8j zweriR&HVrb{IMn*}+i8318ax7f92jIWZ%SC?Qq#XHUT@LsrPG@sb% zv6)G_Tdiw`icS!jC`DK6nqg#$1soobZ@#HmVwm3&&U~%}=JReUf)kVKy~gOUOrNG^ zj8%XUkG^GZlK4nT$H^kq&@l`w5BkYA#u(+9A~kubSi;?Y@|dw>T>0_&pEvs$57Axb zMJ)u7BHmKRP}*4X#bsDC;n|%SLBUPn58F4lT(w>Ae`d7gD053{uMrwQkLlWbz|ch( zaEE1P8SY=Tq;pA&4x}TW|E?@ZsBDyKc)xe`|Gy!s(Qo8-jT_^*0s59N*bo;_maY%) z2oNH|v24tpsXJ!`F;mE`$@Qskn2j>rVD7lCoeC>MooOPH5V1@EH}NU)36ZHV=Bv1S z_?@eiF%*f>j?v9vvNujhZ-ix$2Mla`k!{@hW=WO!i6^9B``@6ph#yV+4A|2apY*^8 z$WPp6%0;02?mp6a=N8Xatr&HHRg52V_YorP+~47m(8hRNZ*if#m}G_d4u~<#Ch)Dc ztaHgVvu}k?Ag(tT>7o`2jYwF|Gs`jD;kF$i7=F*oIv-ZQhYKVKa|}J3hJ%m33PQG} z2!IokcjG1@ZyvA?B))iLz3+^7) z94;2{0}TU$6yl^MR%2NnoQD)0)AcFL4<2!h**S&PM%EZjDm<|d88D`@Fc=5FDse7N zjlYb4*fpl?tLAqqIPPT0l94-1qW0{>*bUlh;QJswKK}0_#$UJ2%muvMf&L)uDN(KB z%{ir#gg>0#-3jT{CoKWQ?k=DaH^AL-RD+F@~}^^u-aA^kf)= zsh7=+YK(_q0;KcwNigz1;5Kz)#k5f+)AF-x`UaLv`=e)9O7eU;FA*ny$c-Og zb{du|n^_&cv=E{UXv3%hG+=zG4}fvmg)c&u+Sgf-MHc&=Y2WZ(sVTmv<{_u%RBWTN z5D)U%V+%sDe{a)|nqy%WnJOF$TgvDq91@@!W9JEp!8qZBB+Vf_KU$`6_>HfUjWkeKHZ1 zJY-t}-S4k+i3Mh2&=`Mzf~72y@Rk9=$1)%QNyq%m%17|!z? zVkQ>R8TJrtzAc0ep)FdG!I)-gAjbU0`DuVbRkl6uBw0u}cGnz*>@UQ6XfQg?z$;wN2i+l3EZ(l4$vK#3jut;9f#BD#c8m_^Vghg4S{chXZW3S<9rWurnm z#|R@Y*3D(axG<=eHRHobqzL|VF1XS&>;I?V{@)Sj@BBvh2SuEh16U@#s12ey7xdTu zv9Hos`SH6KGW-*wni3zO(~7B>_6~e326Vq6AG3AScq5J@_ixZiW4xAraqt|{G|(3o zq({phs8T}ELT^!dIJQF9N&z=W_|&M+K}ANg>aS2Nuc@w`K^|fG*COES1UD{1c+-~g z;js!KQ`|5FKxI4N3(XRs1!H3Q^-7BK34nkObn|qhfcFwc{TMT@-C!31c~dk_Awn|? z+bLpf=(+-E!T6e`a7Y=k$~EI=Oyqz`aATm0@J!duC=}W6^PvD4;Nwd!5;4bk&sD|K z0B=P3+>Go|I+_mp6C}`|c%T`>)l-4*u3slT)Fl;5^5r#ro5hT0&?kzSC`*RIe0o7swkcL~u}PV>S?TAg&&36^epK~9 z#vRuCgwRKLK0xFAU=38qv#fajcRqmB$*T%5h@Hm5Q`MRUn{kZ9Ww&mj{L2dZwNZTl zio3j$&>KSFIf7HHlKuE$_i~{Koe`yjFx%Gsztty_$H_>5#fx(USnpatMs>lYjG9ot zhhESYXh483yxvC)mpIDAauwL4A|_!R_ii&;oFg!*lz8j?bu1~dWEVT<5V%(elClzW zhRO!WZ!0n9vv9Yk1%x+FkJiz(aKUfI2v{R9zIlw`7C987+n5>}Y^T0!!@5qTOX++= zK^{Sj|Irtg=$t}Et@O(yyq4&iLfA6E7wd2ZF*8bI#AjckI*Ubb(tSi;t7$+5mVAyw z5ynjfG-8a~dKpj%IkD^tN})LOksKXepXnm>xYrB*FJN3VV9K8VHbSH6*qj}JpY+Ol zEMf&7AZ1wJCoxaUQ5TvggWG`&qvxr z49RUq_iz-WN_-Cbvfy*y8$KQB^Lq_5L#`^X_(NK6wNQ@u>BYBuI7BNtM|u<_z%(Lw z?{|#ObCY5loDJqn5}F73bH|f5pdJ92>F{zH@3RMR>4~#4M)q#qK1Mg!$7rR-c?#jV z9ZR9O%{1$t8`&AJ5KxVgDa#Qi-ZIfGRx z9ins(X}OE@m1pAfoxnBp_#k(E4@GRRkZufENhZ%Q-hx_gCk9@cd&;uA#l35s@hy%~ z*Yyua&Ep9VPgAIX0qX(q5}RWzuL$E9kyTNmTArI*2B_+K^h4SD1|&RGSc>A2P7gm+ zD2CK55QHL?zPi1cP!y@=sKK?`K1L{X7$MPE;R8vz^$7*vOn`4+sbPxiBUZ!qrf7_s z`o!a26ehlCP$NcoN*{wUj!m+#6QKVm$-<^VHYC2VWsIn5auST5=(*@^iuL*U-~8c= zKV@|1fS7W@lJbadIb05im=1vF0P1Gu$JMA~y+%7e|F;d}wf5}p55Enwt6~nISo^3n zM&zlu`lpQ_e_{4eW-=HRg-ge8ANq#hHW2ZXI$z-gnZNL^A;c4-+#%w`h@u<2Ryx?c zomoDv|75FBlnF!E>Q*v6U|$W}M=^GGa`=35J0624Ysml?bWY&eqcNG=*i&Iq8^&y< zfZ_kSIMTsaLwtxLy|mpdAqambWqDG~eZ*SVbln$Oq4&`qhwk0Kd)E5O_AC^J*w27ny&XX{iF+qshfu0^pVarDzF{lbTm674S#NN|S`2$usnDT(s-qW1RhN zb0}=aNw~UeDiujDhpV)sMJRso86a&u3K7SEl^mZ8BNrfU z-MJKD5@%&jo~5$VH%y|^^}B~jWI`bvYK92KH%#KyLZQQWJ`1%zp`=*%LPY^jzt3m^ zUsdZ<-(H^~0&mU3#oYc66Ej3!>P$_WTaZ#o9xWLC0ka6$Fvj#}?!GcPG;|j9IrLq6 zGvQ)6v46tlXGNj(Okwdclv&A=gD*d+E2TiSl|2;}wPDOw3K;%heQ+%!JD@ri_kW#48&GfW9%||YwDQoDPr&$`Kw!F~~^q}&+`u!Tsp%7JdYtALTM!Y>G z>KxUJH-O_a+-_e)RA1@9uU8_E4-fUrFOfrJ^by`bK z_!oL}9zAA;UD&^H*%U&~-q{~+&bhDSqmY~C;sCBGA*mT7XllTFV+vFjMkYwpyjTVA zs`s|${*76@pqPam){^>>9)hNC`kMGisFUQbzEJSV$%E_z+qj!|4*nV~5y zuQ`iri5CeezIVyYIoXKAB`Dt1h{LrX8Zmy2xci8M&vT8q&+-gA|9?M5HZ3Oqa{YRS z=Xi#F=r_wAE!Df-6VKP)y_L=v(e6<{1E(iOi4ufA?LSNJ>C!kU@!{Ps3@@&Oa}=}6 zeeby>k6V=b0QfQTSdOv9?5WpbFR-U+x&E>6p5xFPUuI*^6Q#B}8~Z!FN%H(Bdb2Cv ztvBB_!?>sJn2=T9R-aay541a-H2LQqkP7$FO?Pmt4cu69(6+_lux8qJ`UCQZo7dF zv9ycb{JC#`>s-f0>rwzI#@Pcw*<4pKDrvdDL!wPEoePlS@#(rub8dtS{snBP|RkP0*Zo&H4a_ZHO0*CTzu236g7L)4UACU z)z35S$0Z!3QnY3l|huKjO`7&oAs1nxRVl!%_P&CrvT22-pM7Iy`?b&VRIGT*I%!_0p*9cZ>=rGFJ9U zV6wf=SyGy=53hSA>oc6G`Abz@#mGPg#F%GTgbavlG7amp+Ij-P6d5E1b<~J z*BZ0a7dSyNSj%mWhZr=BF1*78wlG}GL%asgyMHmqDkw_xYTevQID=5kU0m_})9XQo zb39;u2|*2LS9*ZVy89?z&4wwQaQ6Y9DwXf$EmlfeFtwzry%1E4-@e}Iqd);rNp)CU zx5^SjfEA3ku90HCX7AaUF_!Bkd%zhNV$tX!S7D7pagO#&`A!@GRsw+xT~}}7&mWgn zc{+{BtgNd&7aa3ddu6KI)m|!pbc5>RtM)2@&s2MjcOPH1H>QoZ_SmBolw$w)tYq@n zs6C*giYlOqYIwTKvL%cP;I=!mZY9I8Q9l;wey)COP8xWbemumi@odD>@ zQy6&7EbaK*s2<~$d2fae*P5GBErS%|ac@5Q_7)Q{9=Xjs$1q@B&JM~qi7H*kT8%>2 zAZrJ8tz*x4ccF6q{{X;Y#RKoE)oO6#sN2+Scu>fxeEPuOe@`D}s=ucXqV;+(zU=go z3x6mc48k-_M*wnn)m`AQNK&NBmDIFD9vFA6BFVjoMn*6)i*mFv9+a zC6!DSi_guupD!U$^eresg!dAuU`2eUCGsA|Y-r{Us`#mdh02ZWlfK&FFzobZy z=bQJ}AvN#m&10BztB$$U2JbairHsI^YSWT8}Z}8U$$R`Y-zI zXGS5DMe%5VS4ovF(BQf*4gx8FTiFiR3FN%;m?2(w36CsUgOEJt@eZV8IO3<P+SBR2{~K z7wF*Wo~_kHct^`bZ31A%`vyQQAC(?om!}zOuI*RRkN>$Z*7*4E85C=)6n2|XtWDiQ z?j>+RrO)8ocrJ{afPW5zS|M>8E>~83iq9TKlE8oAh;g)B-;o$iVys4b&dQi-1UR&7 zAY^flkRmr?tQJgs?F9rb!a8D%LG}E~b6Taouowx*x6k#sr{KvdOe2L4b+=(0lnbDA zyZ*Ts5q+-kpc{>fMY_hh|LYbh5^O+>p6ZBfm{~X>qSD9r_(#ZZ(W&bTZn(goPmS*h zG1~3#_`EK@XRWT6rn!b@(bbm?qwKMH6$UbV&VHT8Q}poBQXpH4@yr#kc26Mw{4E)VKfhx*l86LA9reEhhl>hAd|D6>+et4Mts>ls;l=e_8##7D#m z$s1_!UTYhEb)v{{UE{6`SeYyU(vq^1UenrAUWQm+Jpzv0kp9y4L31EFF}`~Hc$rx^ z>+-X5Bi3U{UBF~@VH{?op8`TK1{2uJ4gHh*V&BV8uK$s*3XM7^7a|LJ*jB0M<}L*S*GY z$12o39zwd_dn`TKq9|s67QBUAn_w1(%KS?>7ls&~I@MyDe?D-r#<`MeT|0m$zsgZa z*uUWgqBDT_-X)PM__7Tm1^)?9YW^>$)=4qQlqN z&8>OBRN)VQshj&Roj#&J`kMlkOOJ(-su<0fXv1*H^Cu9a9b@RWDjnKre_C%(YI;8Y zxH3hW@cG0HRpJ*2`Sxn)p1!?z-LV6ygBNX|6EN`lM4OV2uQBWVD)aJ{rw_i4X1*!T z%Yh7)Y+^bjExGwQf{WYdM!Mh>pCO5{i7get28{Q(Eiprd#QZ$+xg>Hh#xX@A6g(t= zWe)wnxuk>5y;2!wXg>TcYfq_}P0?Z6u78lLkmJ6w>mSeYnssKun6mOyn#d~ zC928LbyNXrz%i;>xzW7x>{=Hd$2RkFt(x{WdE|u$6i#;1qY|D*yGIQ;W{ZpOc)Ay` ze`nGS9i^E>c6%2uAuyeEE<#Tp30_a>Kd&*w!p#vYe{{}Z4H>2Z*i<5 zpt?OFE~Q)4RrLTae>N_qa`p0+@N#>Kgm=bX)g^rWuOz(nUHh+uk7=9^?-OxFY~lJp zNqF-+;$I0L(sBv!HOFfb{%3Q9$#a?)TR=GiWn#H9}tX z8fcqsW`?^#ezh6jux%z;W?yYq?fC;LK$oJ!{FM9J+{*S*(P0BaixRsA#hZnSvLICX zmi}ydMtDlVzzd@v{$i}cpfgu}T{}zlZ?0?MwFEduW!Ll8by|J=(*CaNGT^7J>+j1z z?K3kzCC?_|x1U?yF9o!8{U;@SjF`XreEnBStMRW-fD?H*Hlqk%P>96G$i_%nl5{Fn zO#Kk{n4fHUNC=zd@0fW`@21EV;5M7!KC94A4l5P<@xKQFwZwcvp&#i*ZU40F{%Lh% zjPo2W<=0pQV#_gVUcR^ten7yWDR}AWDa#{XJup-!KBFb zp;*Chb&gSd2FADw&&pGU)uyPgF3X{U1R-3RGs7L&6g9KLU@EBBjPeZ7Z<}4n835Ke zFB;&P5 zH|6Jstscz_TUVj+E#Gg}`Itsj%EU)4sU15@r6|_+UsV7VSJMZ|;{c%?vBhCPb^Gpx zFIx(+XLe--z|!vK0S#S?TO91+yTz5;rulkOQN;{2z}<&lV;){3#e@F0^;(xepvC0a`-JV|G77LpOtyWrB%OY@j{=~& zP2N6+xSMO+$J%hQ_}xC(h3)y67Up;Rc+V;Hg6}xTGaNDZleUj4;3iz&f47e^;Kywr zL$(l2XwhQ}`EDQO_T4_d+eZcP@zv*SANbe4+eZ!XL$;4%TM{7ty%VYI005O3&0|~! zM&o!%cEXV_4F7jSaL87D$48A3CYl!jf84_WSO32|sL6$j`Qris_aq40UUyXL2bKMg z8rT2hUDD%EEX=o(4}t1?Ak~we?GnQZDp>#kAOJ~3K~x#lFn$2i_DZThrKZbCS#M9Q zIsw0ZaV@)RtvnqoJ@2_@1Qs`!s5`ja_4@l)>RNU`@%0j{rQ8y{I#qQ|#RK1ZxqeI3 zRRHx=v*H84j3w$=@wwhYb-G=C@^UhQ_@v5n74dm}%0+|}++|9Om@?BehINF|H7NhpfW1c~Z`N2hJMuB`Q8 zXZShe8N)`4^bCL1@(0)o*Yu_y(lL*(i@GQ5agPtL)IA^aRwZy|j)|v$QpKCYt8FQS zU;7^SSb5Yo*?z=FK78B@r#&_b`2+07-Yt$PZlD%3$nk>lnoO=BX1Du2R_DVE|3fC% zhaAL@OHhtstl&Iu&vz4RXzU-8f70nS1}09wFc3ZS-J>y$0dE!R+^TLNrcuHxNT0Cc1m z7A(T}3n;Uv6_7LmFmrH$eX)04(^dUl*R$7k_MVRbxMeE%)yvg6>K1SD{hE4QWqqNz z2}g`lC~}No)s_M2(lHlJ#x#i3G!wJq*}}*8{}|Q8u@aMHo5H@^6ygu~-lkOM`g@y_ zk?%h4fIr{c6qY|$hPU3P?6Z5pz51?;67gZTDK1E8w>4{BbNZHjH-!hKZQzy0{tw?P z4MqWkjplo$p}>d>ZVxHB4W()$#!32NU)R|&CdxGuGi8gxoQ#l%w~)e8P_CKu>woe# zmX8+B{UAEyz7F`oYQJ3_M zv2fwttWtgji{EI;UWm)n!98uxi-;hKgwc0^j|q;`cL{KkgnP8I21rxr0ZTK301Ilb z5#vBWD&}FP5}>Y4-ah12-GGrjkcRA1!(9LLldoKU(m!`$+C=d)R)V7`CB?@6S)> z-iHW9KyU|ei(mYHCV;n_aEI(LYHU*dn)z?pGbcXG`xX6%h4C()`B%}|#RV0%Sp9)u zl%py;m+K`zsF)m;imCSH)fH213V#Z*K>ns#w-oj}{?4v{AT__llmV?6ufI)__6eE- z@UeO`GWz0m?|P5XcT?!1_V+s#_qS92?nXQr0gv!OQNneC8^})G9^*$eJ~uN#d>%Kv z1tq1QYmjg6W0OFb(j5V!#`*UXV8y?0TU>r_jz=&xPY{d1o;z)1)dphjwMs}Hu)b={ zV}ygeauu2E3_tnXi6xrQ@z&B6mxx4PsgDaYE9+V^2F!w+$VUC&D4=95MESfc0PhfR z|Hb%L#3SS?4B$q)))I5u^=+i*n&p>RF>=p?C;)$dCU0OQ_K>gXV;K#AdSY^^!TQgP z@dskcctZen7^_{6S5rALXx1aw%z#xb6=)emNFoMJHM_~p(zruScEs&p2^(Tby)gDF zm?&{;@X;p=DmJ7ukBkNqu&tw=?F=Po2mBE0T6%@brYVAXe05CRq-f4#(hni@yVV6OT>?;_N@M&Zb*KO#iBp){PPmyektGKXYExcW zPO0`933t*Q0-$3ADDv&Szl1A;noPM;D!+MeM$V;@`I!}-MQ?NyjJdqGj8Q6DumYsk zP>vaSn)g%)1xO)~o}USUl4`-7;x;=aZ*;DD!%`?k*)wQjlHo(lb-X#D%t86i$GAAfeGHa<5X-;^PiWhrC!7-k0aQ>x5? z!lbYqkgnl?R(^Z|Fw>lTlpPep75<>W*vX!k=EWVU3iG#YbdnUwb8d{UOrWbYbIgZV z6=FJ`R~pucA&}Nzi21Mpg0%`OLone_B3l%IdDl9s$8W=rG426>p)@8tMJ|P&ei!G? z|BjKEbD9}7U=b8fVKwKhGI<3+p=#0Sz%*8+EKChEAK&vZ>{qzcQ1jk+$Bx9myDryR zw`lAbh0r2;z7{$M&Dgak~@t5QBW1}a8^5d&G(yafpWSqOu@y7?hXHx06$2~hQ7MP)HY-U z!^Uurh<$FW2P2-#G9VC|_Ns2LKazsmJh^K=8HgB=h4Tz<6FhP1#ieG?)Pf*`))!lI zrZ3oCn})FKaD{sQb8jpL(FctKG-HHgWzaTZgm)>Gu2->OS{E^7YV7wj61MhY=#w1{e zo?F-4@#8;T*BnM3uxpH6_M^J%`TI=2Sc)^4?=!rhhKkv-oQ#PY)7yZqJ856sOoGd; z!u-2~FQTKsIKv6J27(7wElPYWzHQg*v+t_kla7o_(Qq!kbm{~WP9f7ep1wJU4$6~0 z?QOsmjwW9$me~_6l2X7aGFv{3Jxf`FYSI&EFUI3S3Hng>2oj`TT$R!bF$>O;yc>S8 ze7QL>yCBu~7FJhPdc0^9r3bFAPn4bzHr2o$zi?{O6U!TXg!BZe3$J5I}NE@(+uRihDCGQFb7RmV8ZDa{+MO>Z`tqTr_etKEKYeXZ#gguTW5@ z?pG*A3q@HbCT^PLwsLdL-DaJ?yDQ7RrZuD8U4NABndwzIM(kYb{rZK*@OO7DGyDw{ z>34VK0hJj4rciu$SFU|%m_%ALutL6)JF#ZL5d?Fl_Y;q;xuhusCtuzTV;o;v4R6?+ zv>4hl0YAc{D z-+c_{RfP?sMpe*N#RwRwRNPAVJmdEbE-83i+&Hk5()Q*&mEn ze0yup>`S4IE~M$c6wx$m%T6tsW)ZMvaWw??Jyss0ILUVCq{e)3Q3Pl4a0CARu8CdXDZne=M z0&yz$01F4JmBw7iez5 zH+Pa=a}R?&%BqupBrQG9V;LiO+LWJyR968DL55M?HIgzEcd=dK0{#61VnkHZrk4qT z2u*Tt$s0Uz{8D(r1+BOWwOyaWBdc+}NL`n2hSwEj@2k2-ehQEd>75qw4_X>StZ2aj za-XNY#)|kTPGOOc?(w{hRm~aTJ2M>pFo)qu!N;82qcj|YAXDK)nL>-W$~>7k?|>R1 ze-J*{@;PVbUiTAg=S+Iw)3rtEVI5xm(!)GAO0RO^7@DqK&V-TJ{nua?p0h#b&sx~B|qSD9aF$TAp-?@SxFfY(2Jq$uSZ#P{d z>j}~>r!>2FiRMgPwV@E20Z@0-ori}e3nkOxTEKZxO?Ya$9)Y3$9y5DTdr*C^6p{#Q zMw~)A#?j7wwb}z?L~QE11xPnxC-5>66N=>DV1bDp%PD*n&Jhj}JW#a&w8wl$)KV?^ zG66T{2Lfg1k5D!#0BZbZb0dc2!oXIcfP!#5g>vLMrbn?UTt2ZQ?qvakoW7CSt%3e`az1^wVL51(WU_S$Ku07>wmdyZ|ntT*j)G3 zVZF$i5IbY(Ivf(haaHU(&uq#Iv)iIbl8kQyuBD^N6()R%r*#~3%Q(4M-xJlinRS+4 zCZF{r!4i@~!v`k%VRTdz@OzYp_|Uic>%3dy7H|UUx<2p9njwKib=D&tQbD|s#qJL?L3-2-H)vdtyCV(5QqR8sdB+R)(4nSl9HY%& zujPPen&Scir-#Hyjz}BTt4IRISOp>&E4393V0giY^6|B zE&iBncfG}r^K6R7F@|dHG3@IpV=P|OierqQy`}c*;#g47B$e8A8NYET384FuQ-0R2 zOSgco*&V)F#$5jDjv6DAHo#=Yn4DL?y2l_J)%KIzxVO@U?MP|S>$;d5%R ztwESLPh0e8s>kSZB|==&5*XO@R0f?ge($} z{<3);tAO2dK+W#)Ddx52VyKPb<@XC3*>g)dxVKcg{O;_>SHBBzdF>IPKF0U5&74;u zS!UlOK&`#y5g;Gq`-vGoYJS8K0EUrx$5<+(-4sS*I8+3~I|!60J?gI4$_`@{3g&8P zFvcNcTvHu1L)X&j%t%Z*3AqJh7}Sthh2eKi2fF_K=E7+T)%v$uDSJL>4RB_q*zk#u z=Kh#t^O%CJz9GhBk;dt?Hapaf)?fw8ofMQ40JjS{a|sxYc}Sn-1F)X2nZxz`F~0ch zD%}lP)E?#I_ee_l_?jbW9%t{f?N*Lz#(|L&^Sb3~n!?CfP<4Ts86`1~HM1kE1FA7v zk13$27`Y3}Fs2pje)CQ-TE>_pS6UK!jBgMzj8DS9@g2| zVV=eK`POxPCMTbeNWF$1d_R01JJU?bk~;33WwOJljZ{BcEUa)0RZE!Bz^J^q6TsOmarcw>dCn1)bEW3^)}lhcN= z#PQZKf|?FN3HeXv{OkDSU3g_}|47!CPB)*=6}<W!{A7S@}5~ zZw~Lf@q|hZ${|uqiVQKC4m3D~JPj0UFqvL$oDv*@!X!3+jeU^6=QPmigI6?rNwUT@ zqhgQVajwS*oG$tB;y;>w%`4Df7fwsg@-@dH7yOSi62FwrD>X473n3lP_%jziQUG0n zz&|7oAQy>fs5CworFB%QU7v=okz5Pi=YUf8jRhFOJt*$g&PTuo&iHS^sJV@6@Xp-J z{uG;^;(AN#>S0G*Z)sf=TqJ`72I8B5yAg9sioxO{IaKZmVw+L&CTZpobA1k%n&1@U zzsLdU8jh=;~Y^qnemAQ<;;Qm@yhRD_Zgd3GXZRdmf;1GpfH5ULxibC49ZO zqwJbFd}l~FIlTIlkI$=Fcjxy<%(b(H>*eRI#;^+TW$rL?07#*}H*d824+g5G0|4;? z9F*s9!$EwigKbbk5?0F>wym~R$f0DOS!_!sD{VD4FV_r3%5Q6J7!U8U&x~St{w|q# z+=Gl1G5)ycImn|O`ISMq(ui>qvZi1$<~o>S6m@FASXU_~j8$|$rnH#%5VL^}ja+$E z#Z12P{R%~;eTY|qY-t?kO!t&7x>av6{ijn1UC1R#Z%_Q{$2fsK&ohSaAqv`EdAAwE zMZnK6hGFgUkWQ5zm})A`JITB*++Xj?=x>Gi|DRO`ca5S6pAB8rnk`^p=i;{90ibYURq`#W< zt4Pu6=hn57J?~ieTEZx0HT(8eq~-SWRis1OTtzyjkRKL7R*~+a{#B%iCEU7Jvgd1& z=F-(b%=xQGOH1MBt4NC&VI`~dAX@mc)$2O4NKEDJ0+)+yTr)}I49yx++##tk;+2rf zhTKEtRDY3a$#zk#gn4BrqS6!j)W9BpbrOon(4l`Kd-Cm|g*~wq$EYLa)rD2!a%J}Y z7WRN$-96m7uG_l#(hj{C{Xq9H#nm6!LMW!?;dZjAk9dWiSZ@Pf8w}y0t9u-E4WzC? zTR)y@Wr{=$2@hQhL^KhjDud99(XojuF+c&!m6ceGBUKjXfDgrb@(w_;9u9 z`V+I*9_KBWkLHROU41n&_hCkXn$ffz04;7$yTV#b*IRhd!%$>wawSBp1<+i7SZ4en zQ&))Abvp=9aXlLn$x(4zlW9Xr$jX3bjOAx4Y#8}x!P_$+c1CFc)fn+MEE~gA_xR@$ zQcW#ycfDBmWE$n_?ZX(EzaLYyD&=Dyf*o86RTEG>z#`4o?u!)wUakW$zU6WaCnTSq zK6<%QA7DS#Qz}49ngZ4Eupdcl+|=uy&v4ldt;zG-NHD6i2YH5TXNFk_u%hNI0-7=2 zhB-8xy$lE1jq$2U1!yRR8kS56qo{je$(#wghXiwvh~|E#?uj+VJbRR5{I7M-z_-)# zgAc#KzLk~I?>8H6r8H^^Tt+W0i5DL#urn-4#^r*msOig=ea4{z$JxU4hrSGmai#*} zr|^zKEX~HHkZLN6?p!PsE9gDGv*|n<4p1@SQ;OaZ&iDcDOxnCx+{m85mx5Vu!gvTp zjZdfWScWTvnmtTByn-KBQaVn3@DQ)Yc#2FOhmJ7{+Zy%G=Bwj%V7zdXX$oL*x+1zE zPU(TLV~ei%9pf&Z>qd&=t$6OOaVc=76Uq!w;juHZr?rOf7Dni{n(#9p2qaM5TEmy$ zvS|(9JN6}{=Tj!s@a>nL3r?&ZAw4)|LzEsWKu?#RR3l#qBV!A|K3{s`x$u?JQ{oC_ zPjT__fTM15;TYw;SZ`B&&~b0zfiJL73bPGMsNuMW%l>B#*k+k}EbMiQK8Oo@fMjZd6810vGuiXHypO4GUjbIqGo0t)hJIy^0Q|m?l{b#L*FhWe;eWJ(igX z4%H)c+aFU(&EmUG(je_Lw!mG9>=4R+NdbQK*$!O`qT4>kvw)`yBVb?ODUKT>*Nj4; zQ5NAh1dfMUwaVi`p)6?Ep)aZ|0$^2q)I?ibTwQQ+E$QIBcw#nq(BhwY9=*8YDei3+ z7i|DgF-Buk%ypUS_L)g;4au)Dfoj|{nYXNK%?+fjYi5$%f1}q1C8zXfjn&XEmdoET0XG$eGhbMT3GmJ4nImR?@Xd4#i zJISM=L66A7x`h+q;uX{q>irxS_`7})gzudPv(R;?KRkaCQO|I_A6E-F-XH$GE+{ts zy5RAV0#3dz;N>C%WS+e;vd1aXey2 zS`->+F&|^*NBRx`DluwXeoz3)Kij!fsyW?}yJj6y-rk~`Ef5Jl#9t{CyA@tG=mRnP z8UNuIf#SG!m6ToOP`D;yEJj=k97!@xk0LwXz-HOEe|ds!hH_|cF~Qg zjL-x^fVV;^U6Wa;Zo|{^twP}st;;D%2$Bsb6pSo9S`yq>3WavAUu4;<8}eQ8F&B~H ztUk;buFQZkSj~E3*wgoxeQ4Pg#K4Eh9xKMA^aMa9MvzOopd8Y09B;`b%^CUd!BMJz z822MC$P&sC&M25f$Cwh4%VWIV@aUofcv-mwbwfoW;48R9rCH0Xi@GcnmB58nPW(x;9?koc7X{%9F)dg{1}az@hW-zv@@ z&qjk1FnI^_ia!}2FpaxVMDngj`-^Z?xcJT8)HQrJA>92g@p#=-t0lagP(1UU>*q2* zGiqYAi{Y%icr~31lU*M}e2?jiz>2f7!~lQ02PfH&_W`IaK0PJj{S6A##2ZqsYh}yb zIQ@F?b#Va5%?;(O*uWTM@6ykQaxVGy%U>>A_a zC)zWay%*{S#u^%+Nmed#<8T&alMg`M-o%X~t9vLEB_;l*LgD{#8TvKAc)9sff6t7p zowzT=u)*DEA`$nQSBwAv1hGj(K~yEI(tVpVy(2`koG||AuB16 zeT)h(DLuL?&RB!MfwP3r3b3{!-%(!t5oqW5%S*)Qr_Yo6+k>YWPowWTcx0uZp`RZZ zLnaqbH687B%mHhV=;hX~0mj?yF1I-qcYzzHJVre+fG@@Bp=yjm;VJMqK-7kQE|W~I#re&&*Zb<_I=s5FEM~}=X3MH1wox(&!F?f$7~;+n z(e~A|M=g(1pCICVZ^kDwxY094oDL(InY3ow=UPa9EOKt@wAY| zD;XZ*1j{*H`z2|-QXC<;2Uxtvt?|MX7S@Wr>o|quF)D?kIL{4TUt{kgS%#UJ_|mD8 zJv0%Y{!$}*hAghn+BCi0-JOPw%@@h8u_PFyTPgWc>M}A3y6>_n)V1RP8!#51m~Xly z0k|2!LefxZPQoRRCdHedGzIs&}A^DDG{E5@=uK@ea#k(I$~^mBs8yi&oji#7D%4vS~$M0s|8R=jGjHO<0Bxt zT-WuZ>ZJ@j>J~O>$Fo{`(Mkc;4OTd%s~YOl7pI)8jI;)dA1#bg*)^!OQKZR=5vN`B zVpO@!G9M{f(oKL`3jpt~T;?^j>{Wm>YxtJf_*(W%aTWllzCX3cQ+!fbf|zB>plL%| z{e@t(7+PO~CX+`|Wq|Vg2KKJa-fL + + + + + + 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:2594371469.8 %
Date:2024-06-08 22:13:12Functions:10712387.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
69.5%69.5%
+ 		69.5 %2551 / 367185.6 %95 / 111
<unnamed>69.5 %2551 / 367185.6 %95 / 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..6db491d47a --- /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:2594371469.8 %
Date:2024-06-08 22:13:12Functions:10712387.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
69.5%69.5%
+ 		69.5 %2551 / 367185.6 %95 / 111
<unnamed>69.5 %2551 / 367185.6 %95 / 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..57c56cfb5e --- /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:2594371469.8 %
Date:2024-06-08 22:13:12Functions:10712387.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
69.5%69.5%
+ 		69.5 %2551 / 367185.6 %95 / 111
<unnamed>69.5 %2551 / 367185.6 %95 / 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..0bda6f76ec --- /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:2594371469.8 %
Date:2024-06-08 22:13:12Functions:10712387.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
69.5%69.5%
+ 		69.5 %2551 / 367185.6 %95 / 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..2571a7ae8c --- /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:2594371469.8 %
Date:2024-06-08 22:13:12Functions:10712387.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
69.5%69.5%
+ 		69.5 %2551 / 367185.6 %95 / 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..5bbd2dcf3d --- /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:2594371469.8 %
Date:2024-06-08 22:13:12Functions:10712387.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
69.5%69.5%
+ 		69.5 %2551 / 367185.6 %95 / 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..9af1fdbc05 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-06-08 22:13:12Functions: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&)107
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()107
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1356
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3997
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
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&)123712
+
+
+ + + +
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..a23a7527c9 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-06-08 22:13:12Functions: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&)3997
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8372
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)535
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1356
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)100583
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)4010
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)905
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2786
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1168
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&)123712
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)107
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()107
+
+
+ + + +
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..b43baa5885 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html @@ -0,0 +1,154 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-06-08 22:13:12Functions: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         107 : OutputPublisher::OutputPublisher() {
+      10         107 : }
+      11             : 
+      12         107 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14         107 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15         107 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16         107 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17         107 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18         107 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19         107 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20         107 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21         107 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22         107 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23         107 : }
+      24             : 
+      25      123712 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27      123712 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28      123712 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32        3997 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33        3997 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34        3997 : }
+      35             : 
+      36        4010 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37        4010 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38        4010 : }
+      39             : 
+      40      100583 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41      100583 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42      100583 : }
+      43             : 
+      44        8372 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        8372 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        8372 : }
+      47             : 
+      48        1168 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49        1168 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50        1168 : }
+      51             : 
+      52         905 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53         905 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54         905 : }
+      55             : 
+      56        2786 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57        2786 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58        2786 : }
+      59             : 
+      60        1356 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61        1356 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62        1356 : }
+      63             : 
+      64         535 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65         535 :   ph_hw_api_position_cmd_.publish(msg);
+      66         535 : }
+      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..2989caef01 --- /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:37453769.6 %
Date:2024-06-08 22:13:12Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::EstimationManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()5
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)5
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)5
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)107
mrs_uav_managers::estimation_manager::EstimationManager::onInit()107
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()107
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)142
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)343
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const343
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const686
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const3568
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const19332
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)20555
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)191650
mrs_uav_managers::estimation_manager::StateMachine::isInSwitchableState() const191652
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)191653
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const212194
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const403748
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1840363
+
+
+ + + +
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..fa66035108 --- /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:37453769.6 %
Date:2024-06-08 22:13:12Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)343
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()5
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)107
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)191650
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&)191653
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)5
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)107
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)5
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)20555
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> >&)142
mrs_uav_managers::estimation_manager::EstimationManager::onInit()107
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()107
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const343
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const403748
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const686
mrs_uav_managers::estimation_manager::StateMachine::isInSwitchableState() const191652
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const212194
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const19332
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1840363
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const3568
+
+
+ + + +
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..81147f04d5 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html @@ -0,0 +1,1338 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37453769.6 %
Date:2024-06-08 22:13:12Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..bfca8e65a73b48e7441f2df02cb1b8f2286dafbe GIT binary patch literal 4355 zcmV+e5&Z6nP)YvKVy8L+Wyg_&HACO~`FGEWR3 z&pNLZXXHdNil~^b&44zp%`{eYZNw;ceSPDHkdaTb97X;$Km=R@901@Y#tzdsScc`| zereZQ-yOQvq>TiAN8h_sLII!=qn*ZGI9@*LqMe|s`_NIETm>UzZmbP}*AMoMnQ;X` zJH`V4=HWMMiH*S2wH46ZwE=G}jb%{tENYx=avQ9?nA^n!Rv;Xg(5B<>>|V_C0^2;_ z#(6Zgn+5I3gIgL`+T@*K}REe1> zV`Ir>Ua`)M*{o(Qq#fh)`Fy#yg+J^2&w4zp?#;dJfO}hCdE1_#d5mwkr{3OxTLHua z+O-ELg|Kq8h$r#9NfaOAfg*YNuWe3_t9x&K1;CYvO~hyt&&9T49HHvUl04*?yb3UF z$qMGQN0E=0&UVARaoYAegc3291bF~j6wNUs8WPO4$*8mnJeOZd;L%YDPp%gDYb6wO z0Rg`Z|KgERy=Gx5q!-7N4kPnx1ulzQ7C4snM=u=fo9#6a+ZV3S33k^<2-1_vr{G z%*X-taWfhk&WWw;dMZ0-6>;20hcXhBau6#Ah`TNZ&VZU}DpAArC?g@_u`_K=3t6fVnd|t>3RpXl zM2rY)HAagVj!*AdUs?TkW7ir1xa$Y{-q0Y$tZNwo?fN|5lxwBt>qfXf$1~8L(!rDX zu9&7Ab|i1WfdRaitK=_e7c%3AAqVt)3eh>8Z$!}pBDFXO!s-Je zT5)Jq)YaG`*R4GAY=gRZ1+D^NpSXG1hoI%=QsGz4iJbg!1Q%(qI^zSH23U&`L`hlf z7jR8bp2f%>8ZcvwlTejq1)#O-!>0)D!+i)Y(C!A@hZxy<`xIcTGxp)@o6%5=H&$pY zjpoVKc*-|hMtYv|y#thYZv%n(Z#*~rFj$rR1Pd6zR4uyHD6XMGdkiTJ!)SLqKaM{iDDQb0nIIs$_Lv>=Zoz^S8j=8I&pr+o zxZI$AWd1yREC$xI=luJx^3DD{`#;aVU3&yA`VpS}|8wO!BOX>l^6n!GA%nXT7)45k zH0PR^6b3xL%lqtg_2COyHF$w%{qUfbQCCZjCo75v{I_+DFk%^MTw`IM?0SC{5$ZIe zVb+PPhkxEo+)I*=5RsT3^$Mr4d6Kt z6q74VNx#RpL>1JGp%ztu8Na7Z!>Ry| zg<2udHD+gaj}v_9<(PWbG1>|Sp8H~$wx9dPRM=@g;qEnU#8>hL0#p~xr~P<(hlAQ^ zcFp8@ya;V%oii}{N#NFF)z_Cq_x4EX9moK%gGs>|59vfSv#<+Yce%pz;)N!UAk8dK zqCQfm9ExYs4#+pTT;efXkI{f`tP}WNaX!RyE2ZG{{s?7Q>LAI{Yb;55&g7MV;KOLL zr*Ip>;xfVrBbZnG#Z$CTXQ$|Bfm>Kf%PsC^xV*z%jzu;B*aS?8M%6&hfEk;jv1{QJ z{L5*MOCnyc#g3htU40=)pI_bZ<+yi$;B`2(-aiQyz*HwDx@VrFQt&!KRpVdA!?E%l zG~#=ggzJ774Hszi8XldKm?7A<=%+9kL1JNrX$5!Vt{}oK>)I&C4%a2v6A(z?8+-|b zPq+>_mo>SH;4&hJaFuRuwu>hpnC%rW?gFlAkE*7$d^8H22%WLvd;m>zG-(0L=N!#c z0R1)M$(P}eiAV7%vKpk{&oy&Redf;aG>*9g;J3q}I%7Hnc5^O8S^$|W*WqN9F?xRu z`4j*_J~AJ4DLU8AXlEkz+Lj6bDGk_CRm;An1itV{dCj>-7a<*H^t zs~pkfQles5j_dK8N}z0Z_494(BF9dirVnOm=DV~Yg;oc6?G2}F*Hd0e+qA?DkGR)n z$iB|maGP0kV&v#AB?SyhXIy7?&*X$NEZCL~wIgIDH z&Z~y1)U4aQuDepFgfZm*70gAy$BSPAq0T-&c=P{CMoa^mlk1Q!8 z6>-i)Ze(8$m)ZKlD;@B_EF7~#fT8kHJnlxjE&_Ut+BOA{C7p_E*>l~8aJ9whme+cv z0xR&mSa=bp1U^-nMu-tla)u{_#Cz=li*`*ttr=$J%XIw@q6%Qk0p!tVUOT0D#2Yyb zrde1x1C7jnhdCq84&ed2pkdU9P^=>P^bk7EsL;4#ciJ#uJd`$v7Zboc^;3#Ci7&YK zQH{TY_7#3lyqQK|yx_#4G6@?q5Thocs=JyFT!vHUR*Z3C2Lb_NJPVB&VX zaIZ)sZ8IRcVbQ!9ZzdW^2x$&aad4(wgb2pkmOWruaKm9A z5S4&{g*Zh`!2o4uV@gR_W*A<|%xnzCiu{ zvaar-%LNSPLoCLw;Uv7q;{F?J7On0Chp>XiUe-);34K80&zk*jtXa2UL8!xx1s_4b z;+$FfIK%OZY&X3J#`4<2I4<6U&MQdu*EOeIPzC~TJ1}6Pa#Ug@#7mjQh=vC=2>g4h zn&jVN!;{dRgW23fTz$@_Tb`?5B~U4pd;OuRR9rad^@TQHc&f=OvoUyjiSP9glgtdW2wGv37F$27s#zs$SVa@?A@dN~7K`L8l_N1i4 zfVuOL9R~F8_3$Ygl!DdCIdM&x*^W&6%R5WqcX+!n)1ye+$Kb$`5)fdII@ouIB)31) zT!~aZkJB|HKvk$^^T>qJ7rcIISUu2ruEh$rh`uUWGX$#;yqh{hq3oqFTT8$aLpo$ZCJw`RXH}_6eYVmM$qwAJ*fzP*Sk2wpV zRl8XLkB>SJp;}2=S@gA286E{1vy>*!{fv_&{@a|Gx>Tkz1aUVENX5nAj@~aS(x#1$ zjIohrSUv#q*`MdZKXZomaVLNJ=Ap0b`XRrd@^yu~D0nRc9%{ChZ@%q)XMv$0|F};; zrx5_AD~pdu7BHQ$_hI8#aCK8w$~`Y~-Az1msAnAZMj3SM;E2=;YfL`RXQA298 zQWy~6N>&E}mQ6*6SW5koJaRJ*k`3_os+PEIi|BIW``MK%*d2Y^0>A|Z^jO6;HJ8$i zWzy*^i$J4ppz6AIr&!D8%~fe3Rx_eTQbhLa5^mtPcH^a+{vJYqD*1Tx_KZJbvu#4J zp=(5>aM2%k)t>;_!{xH-W3ct>mbU7w-($4(&(4=Rsp$e?bMsO_sltU&NzPj%!Q(z+^)5#biI5HEDoXt;&dh7la8I94fn zn9RohnMe#f+|}{EuKvl(aqTA4{*qMiqv=#39=B-j%S;Inm-w&n0053?+V&|Qu3!s8 zcvt7y6LKgP#DF*jGlXauzikM?rMq!L{IDU^M-U95I)bGkOcU`Nhmefm&lo~u#IC|s xFt_XXg~OdBP%p=Lp2oQ00*wXaymh9{{{a$5dzyTjO^E;i002ovPDHLkV1m_{Lbd<^ literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html new file mode 100644 index 0000000000..f31a621845 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:638871.6 %
Date:2024-06-08 22:13:12Functions: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)2484
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)2484
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const2484
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const3709
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const4968
mrs_uav_managers::Estimator::getMaxFlightZ() const18858
mrs_uav_managers::Estimator::isStarted() const42201
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)313788
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)314015
mrs_uav_managers::Estimator::isReady() const369562
mrs_uav_managers::Estimator::getName[abi:cxx11]() const632638
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const751486
mrs_uav_managers::Estimator::isMitigatingJump()760967
mrs_uav_managers::Estimator::publishDiagnostics() const1065795
mrs_uav_managers::Estimator::isError() const1378407
mrs_uav_managers::Estimator::isRunning() const1744250
mrs_uav_managers::Estimator::isInitialized() const2738298
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const8130483
mrs_uav_managers::Estimator::getCurrentSmState() const8922929
+
+
+ + + +
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..f09ca31dc6 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:638871.6 %
Date:2024-06-08 22:13:12Functions: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)2484
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)313788
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)314015
mrs_uav_managers::Estimator::getHeadingRate(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_uav_managers::Estimator::isMitigatingJump()760967
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)2484
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const751486
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const3709
mrs_uav_managers::Estimator::getMaxFlightZ() const18858
mrs_uav_managers::Estimator::isInitialized() const2738298
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const4968
mrs_uav_managers::Estimator::getCurrentSmState() const8922929
mrs_uav_managers::Estimator::publishDiagnostics() const1065795
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const2484
mrs_uav_managers::Estimator::getName[abi:cxx11]() const632638
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const1378407
mrs_uav_managers::Estimator::isReady() const369562
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const8130483
mrs_uav_managers::Estimator::isRunning() const1744250
mrs_uav_managers::Estimator::isStarted() const42201
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..c9a61bb36d --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html @@ -0,0 +1,295 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:638871.6 %
Date:2024-06-08 22:13:12Functions: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        2484 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10        2484 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14        2484 :   previous_sm_state_ = getCurrentSmState();
+      15        2484 :   setCurrentSmState(new_state);
+      16             : 
+      17        2484 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18        2484 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23     8130483 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24     8130483 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29     2738298 : bool Estimator::isInitialized() const {
+      30     2738298 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35      369562 : bool Estimator::isReady() const {
+      36      369562 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41       42201 : bool Estimator::isStarted() const {
+      42       42201 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47     1744250 : bool Estimator::isRunning() const {
+      48     1744250 :   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     1378407 : bool Estimator::isError() const {
+      60     1378407 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65     8922929 : SMStates_t Estimator::getCurrentSmState() const {
+      66     8922929 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71        2484 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72        2484 :   std::scoped_lock lock(mutex_current_state_);
+      73        2484 :   current_sm_state_ = new_state;
+      74        2484 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78        4968 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79        4968 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84        2484 : std::string Estimator::getCurrentSmStateString(void) const {
+      85        4968 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90      760967 : bool Estimator::isMitigatingJump(void) {
+      91      760967 :   if (is_mitigating_jump_) {
+      92           5 :     is_mitigating_jump_ = false;
+      93           0 :     return true;
+      94             :   } else {
+      95      760945 :     return false;
+      96             :   }
+      97             : }
+      98             : /*//}*/
+      99             : 
+     100             : /*//{ getName() */
+     101      632638 : std::string Estimator::getName(void) const {
+     102      632638 :   return name_;
+     103             : }
+     104             : /*//}*/
+     105             : 
+     106             : /*//{ getPrintName() */
+     107        3709 : std::string Estimator::getPrintName(void) const {
+     108        7419 :   return ch_->nodelet_name + "/" + name_;
+     109             : }
+     110             : /*//}*/
+     111             : 
+     112             : /*//{ getType() */
+     113           0 : std::string Estimator::getType(void) const {
+     114           0 :   return type_;
+     115             : }
+     116             : /*//}*/
+     117             : 
+     118             : /*//{ getFrameId() */
+     119      751486 : std::string Estimator::getFrameId(void) const {
+     120      751486 :   return ns_frame_id_;
+     121             : }
+     122             : /*//}*/
+     123             : 
+     124             : /*//{ getMaxFlightZ() */
+     125       18858 : double Estimator::getMaxFlightZ(void) const {
+     126       18858 :   return max_flight_z_;
+     127             : }
+     128             : /*//}*/
+     129             : 
+     130             : /*//{ publishDiagnostics() */
+     131     1065795 : void Estimator::publishDiagnostics() const {
+     132             : 
+     133     1065795 :   if (!ch_->debug_topics.diag) {
+     134     1065733 :     return;
+     135             :   }
+     136             : 
+     137           0 :   mrs_msgs::EstimatorDiagnostics msg;
+     138           0 :   msg.header.stamp       = ros::Time::now();
+     139           0 :   msg.header.frame_id    = getFrameId();
+     140           0 :   msg.estimator_name     = getName();
+     141           0 :   msg.estimator_type     = getType();
+     142           0 :   msg.estimator_sm_state = getCurrentSmStateString();
+     143             : 
+     144           0 :   ph_diagnostics_.publish(msg);
+     145             : }
+     146             : /*//}*/
+     147             : 
+     148             : /*//{ getAccGlobal() */
+     149           0 : tf2::Vector3 Estimator::getAccGlobal(const sensor_msgs::Imu::ConstPtr& input_msg, const double hdg) {
+     150             : 
+     151           0 :   geometry_msgs::Vector3Stamped acc_stamped;
+     152           0 :   acc_stamped.vector = input_msg->linear_acceleration;
+     153           0 :   acc_stamped.header = input_msg->header;
+     154           0 :   return getAccGlobal(acc_stamped, hdg);
+     155             : }
+     156             : 
+     157      314015 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     158             : 
+     159      625220 :   geometry_msgs::Vector3Stamped acc_stamped;
+     160      313446 :   acc_stamped.vector = input_msg->control_acceleration;
+     161      313607 :   acc_stamped.header = input_msg->header;
+     162      621152 :   return getAccGlobal(acc_stamped, hdg);
+     163             : }
+     164             : 
+     165      313788 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     166             : 
+     167             :   // untilt the desired acceleration vector
+     168      624822 :   geometry_msgs::Vector3Stamped des_acc;
+     169      313500 :   geometry_msgs::Vector3        des_acc_untilted;
+     170      313647 :   des_acc.vector.x        = acc_stamped.vector.x;
+     171      313647 :   des_acc.vector.y        = acc_stamped.vector.y;
+     172      313647 :   des_acc.vector.z        = acc_stamped.vector.z;
+     173      313647 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     174      313775 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     175      627277 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     176      314051 :   if (response_acc) {
+     177      314052 :     des_acc_untilted.x = response_acc.value().vector.x;
+     178      314052 :     des_acc_untilted.y = response_acc.value().vector.y;
+     179      314052 :     des_acc_untilted.z = response_acc.value().vector.z;
+     180             :   } else {
+     181           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform from %s to %s failed", getPrintName().c_str(), des_acc.header.frame_id.c_str(),
+     182             :                       ch_->frames.ns_fcu_untilted.c_str());
+     183             :   }
+     184             : 
+     185             :   // rotate the desired acceleration vector to global frame
+     186      314052 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     187             : 
+     188      621328 :   return des_acc_global;
+     189             : }
+     190             : /*//}*/
+     191             : 
+     192             : /*//{ getHeadingRate() */
+     193           0 : std::optional<double> Estimator::getHeadingRate(const nav_msgs::OdometryConstPtr& odom_msg) {
+     194             : 
+     195             :   double hdg_rate;
+     196             :   try {
+     197           0 :     hdg_rate = mrs_lib::AttitudeConverter(odom_msg->pose.pose.orientation).getHeadingRate(odom_msg->twist.twist.angular);
+     198             :   }
+     199           0 :   catch (...) {
+     200           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading rate", getPrintName().c_str());
+     201           0 :     return {};
+     202             :   }
+     203           0 :   return hdg_rate;
+     204             : }
+     205             : /*//}*/
+     206             : 
+     207             : 
+     208             : /*//}*/
+     209             : 
+     210             : }  // namespace mrs_uav_managers
+     211             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html new file mode 100644 index 0000000000..c6ae1bd90d --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html @@ -0,0 +1,73 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..f401921282a6e8acd39246b08a7b89c74383dc24 GIT binary patch literal 824 zcmV-81IPS{P)zrGivy3RaagYEM zi_T=hDp4|pU_36HC#XtBn*Fzdv-qzAxc|y)Bo|c!eK^Q%B#{_qT$I&eW}pdRctqA^ z_zcRL<=wJIcS2^(#o7R@SyvaVNy->&lFa*AUySe$?bg?SlQqHpBUm40E6TQihytZT zh08)!E@6eU|NHv2ta*PQ464Y**q5w3%szy5vsppSUgm-|VxpEz0vhw8py-kO<7Dieod7zzZj& z9VaOtI=F^2ywXC~$Pc-1tz32B&Zvi!&^m(zZZ~Z4?aEiK{XjeuCK!M<@9Wr;Wm(a^ z5qmSX^BSNxV_l8;8g0>_ulp{1zpo|dYe{ETvhMO4gEiytwjfd2OiD(V8p+yqmJflA z zkM5gC5EN@QzvF?JZQDBlqM?&}Ml2~iRLP=FAJ=cMbGpSV34X(lWMCACHXqHQg18)) z=kS->(VIeUN9AAQOviqR6&~Min88tv5a=IJw#-030xiRD0+)0RfVvw33yn3_1LyP{ zJ(`}s=MqO=TSbj@nxFjpkaCiiYk2$LRL@#jq(_#8_Sn)ib<8<`+51SfA0=6wF5CZ*pz-UL68qU~hRl zYXbBqL2Us&U*o&&zI@QVll0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-06-08 22:13:12Functions: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&) const86
mrs_uav_managers::AglEstimator::publishAglHeight() const153229
mrs_uav_managers::AglEstimator::publishCovariance() const153229
+
+
+ + + +
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..d0df69a8d5 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-06-08 22:13:12Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const153229
mrs_uav_managers::AglEstimator::publishCovariance() const153229
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const86
+
+
+ + + +
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..71e119d4d4 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-06-08 22:13:12Functions: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      153229 : void AglEstimator::publishAglHeight() const {
+       8      153229 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9      153229 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13      153229 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15      153229 :   if (!ch_->debug_topics.covariance) {
+      16      153229 :     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          86 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26          86 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27          86 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29          86 :   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          86 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35          86 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36          86 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37          86 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38          86 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39          86 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40          86 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41          86 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42          86 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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..3072785c16 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-06-08 22:13:12Functions: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..38512a5163 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-06-08 22:13:12Functions: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..644fb1049d --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-06-08 22:13:12Functions: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..ab23637f1a --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-06-08 22:13:12Functions: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..35142c8801 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-06-08 22:13:12Functions: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..7214f33ff0 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-06-08 22:13:12Functions: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..dfa4640734 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-06-08 22:13:12Functions: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&) const113
mrs_uav_managers::StateEstimator::getInnovation() const170245
mrs_uav_managers::StateEstimator::getPoseCovariance() const170245
mrs_uav_managers::StateEstimator::getTwistCovariance() const170245
mrs_uav_managers::StateEstimator::getUavState()188672
mrs_uav_managers::StateEstimator::publishOdom() const197949
mrs_uav_managers::StateEstimator::publishUavState() const197950
mrs_uav_managers::StateEstimator::publishCovariance() const197950
mrs_uav_managers::StateEstimator::publishInnovation() const197950
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const402292
+
+
+ + + +
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..5c67237f8e --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-06-08 22:13:12Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::getUavState()188672
mrs_uav_managers::StateEstimator::publishOdom() const197949
mrs_uav_managers::StateEstimator::getInnovation() const170245
mrs_uav_managers::StateEstimator::publishUavState() const197950
mrs_uav_managers::StateEstimator::getPoseCovariance() const170245
mrs_uav_managers::StateEstimator::publishCovariance() const197950
mrs_uav_managers::StateEstimator::publishInnovation() const197950
mrs_uav_managers::StateEstimator::getTwistCovariance() const170245
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const113
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const402292
+
+
+ + + +
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..a3ad1a0473 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html @@ -0,0 +1,263 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-06-08 22:13:12Functions: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      188672 : std::optional<mrs_msgs::UavState> StateEstimator::getUavState() {
+       9             : 
+      10      188672 :   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      377282 :   return mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      16             : }
+      17             : /*//}*/
+      18             : 
+      19             : /*//{ getInnovation() */
+      20      170245 : nav_msgs::Odometry StateEstimator::getInnovation() const {
+      21      170245 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      22             : }
+      23             : /*//}*/
+      24             : 
+      25             : /*//{ getPoseCovariance() */
+      26      170245 : std::vector<double> StateEstimator::getPoseCovariance() const {
+      27      170245 :   return mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_.values);
+      28             : }
+      29             : /*//}*/
+      30             : 
+      31             : /*//{ getTwistCovariance() */
+      32      170245 : std::vector<double> StateEstimator::getTwistCovariance() const {
+      33      170245 :   return mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_.values);
+      34             : }
+      35             : /*//}*/
+      36             : 
+      37             : /*//{ publishUavState() */
+      38      197950 : void StateEstimator::publishUavState() const {
+      39             : 
+      40      197950 :   if (!ch_->debug_topics.state) {
+      41           0 :     return;
+      42             :   }
+      43             : 
+      44      395881 :   auto uav_state = mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      45      197942 :   ph_uav_state_.publish(uav_state);
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ publishOdom() */
+      50      197949 : void StateEstimator::publishOdom() const {
+      51             : 
+      52      395899 :   auto odom = mrs_lib::get_mutexed(mtx_odom_, odom_);
+      53      197950 :   ph_odom_.publish(odom);
+      54      197950 : }
+      55             : /*//}*/
+      56             : 
+      57             : /*//{ publishCovariance() */
+      58      197950 : void StateEstimator::publishCovariance() const {
+      59             : 
+      60      197950 :   if (!ch_->debug_topics.covariance) {
+      61      197949 :     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      197950 : void StateEstimator::publishInnovation() const {
+      73             : 
+      74      197950 :   if (!ch_->debug_topics.innovation) {
+      75      197949 :     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      402292 : std::optional<geometry_msgs::Quaternion> StateEstimator::rotateQuaternionByHeading(const geometry_msgs::Quaternion& q, const double hdg) const {
+      85             : 
+      86             :   try {
+      87      402292 :     tf2::Quaternion tf2_q = mrs_lib::AttitudeConverter(q);
+      88             : 
+      89             :     // Obtain heading from quaternion
+      90      401886 :     double q_hdg = 0;
+      91      401886 :     q_hdg        = mrs_lib::AttitudeConverter(q).getHeading();
+      92             : 
+      93             :     // Build rotation matrix from difference between new heading and quaternion heading
+      94      400535 :     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      400581 :     geometry_msgs::Quaternion q_new = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_q);
+      98      399344 :     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         113 : bool StateEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+     109             : 
+     110         113 :   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         113 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+     116         113 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+     117         113 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+     118         113 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+     119         113 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+     120         113 :   ph_->param_loader->loadParam("requires/position", requires_position);
+     121         113 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+     122         113 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+     123         113 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+     124             : 
+     125         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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         113 :   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..15cf58fff2 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-06-08 22:13:12Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
<unnamed>71.6 %63 / 8882.6 %19 / 23
estimation_manager.cpp +
69.6%69.6%
+ 		69.6 %374 / 53787.5 %21 / 24
<unnamed>69.6 %374 / 53787.5 %21 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..6d9c5cd4fd --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-06-08 22:13:12Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.6%69.6%
+ 		69.6 %374 / 53787.5 %21 / 24
<unnamed>69.6 %374 / 53787.5 %21 / 24
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
<unnamed>71.6 %63 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail.html b/mrs_uav_managers/src/estimation_manager/index-detail.html new file mode 100644 index 0000000000..3f69ddcebd --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-06-08 22:13:12Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
estimation_manager.cpp +
69.6%69.6%
+ 		69.6 %374 / 53787.5 %21 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-sort-l.html new file mode 100644 index 0000000000..730f83dcc3 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-06-08 22:13:12Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.6%69.6%
+ 		69.6 %374 / 53787.5 %21 / 24
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index.html b/mrs_uav_managers/src/estimation_manager/index.html new file mode 100644 index 0000000000..1c83fd85ba --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43762569.9 %
Date:2024-06-08 22:13:12Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.6%69.6%
+ 		69.6 %374 / 53787.5 %21 / 24
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..499eb7b957 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22425886.8 %
Date:2024-06-08 22:13:12Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)2
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::gain_manager::GainManager::onInit()107
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)108
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)2348
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&)2362
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)23957
+
+
+ + + +
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..75d7856f43 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.func.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22425886.8 %
Date:2024-06-08 22:13:12Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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&)2362
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)2
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)2348
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)23957
mrs_uav_managers::gain_manager::GainManager::onInit()107
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)108
+
+
+ + + +
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..4934af7673 --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.html @@ -0,0 +1,730 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22425886.8 %
Date:2024-06-08 22:13:12Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
<unnamed>86.8 %224 / 258100.0 %7 / 7
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
<unnamed>84.1 %195 / 232100.0 %8 / 8
uav_manager.cpp +
67.3%67.3%
+ 		67.3 %787 / 1169100.0 %38 / 38
<unnamed>67.3 %787 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-detail-sort-l.html b/mrs_uav_managers/src/index-detail-sort-l.html new file mode 100644 index 0000000000..63b44d54c1 --- /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:1247171872.6 %
Date:2024-06-08 22:13:12Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
uav_manager.cpp +
67.3%67.3%
+ 		67.3 %787 / 1169100.0 %38 / 38
<unnamed>67.3 %787 / 1169100.0 %38 / 38
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
<unnamed>84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
<unnamed>86.8 %224 / 258100.0 %7 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-detail.html b/mrs_uav_managers/src/index-detail.html new file mode 100644 index 0000000000..7ad0526c62 --- /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:1247171872.6 %
Date:2024-06-08 22:13:12Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
<unnamed>84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
<unnamed>86.8 %224 / 258100.0 %7 / 7
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
uav_manager.cpp +
67.3%67.3%
+ 		67.3 %787 / 1169100.0 %38 / 38
<unnamed>67.3 %787 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-f.html b/mrs_uav_managers/src/index-sort-f.html new file mode 100644 index 0000000000..7dada22fd2 --- /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:1247171872.6 %
Date:2024-06-08 22:13:12Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
uav_manager.cpp +
67.3%67.3%
+ 		67.3 %787 / 1169100.0 %38 / 38
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-sort-l.html b/mrs_uav_managers/src/index-sort-l.html new file mode 100644 index 0000000000..6b24779217 --- /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:1247171872.6 %
Date:2024-06-08 22:13:12Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
uav_manager.cpp +
67.3%67.3%
+ 		67.3 %787 / 1169100.0 %38 / 38
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
constraint_manager.cpp +
84.1%84.1%
+ 		84.1 %195 / 232100.0 %8 / 8
gain_manager.cpp +
86.8%86.8%
+ 		86.8 %224 / 258100.0 %7 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index.html b/mrs_uav_managers/src/index.html new file mode 100644 index 0000000000..9159cf4c7b --- /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:1247171872.6 %
Date:2024-06-08 22:13:12Functions:647387.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::NullTracker::resetStatic()0
mrs_uav_managers::NullTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::NullTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_managers::NullTracker::~NullTracker()107
mrs_uav_managers::NullTracker::~NullTracker().2107
mrs_uav_managers::NullTracker::deactivate()237
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)241
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_managers::NullTracker::getStatus()7192
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
+
+
+ + + +
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..742f3c1e52 --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.func.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-06-08 22:13:12Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::NullTracker::deactivate()237
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>)107
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&)375
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_managers::NullTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_managers::NullTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_managers::NullTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)241
mrs_uav_managers::NullTracker::getStatus()7192
mrs_uav_managers::NullTracker::~NullTracker()107
mrs_uav_managers::NullTracker::~NullTracker().2107
+
+
+ + + +
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..bf102c0d6c --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.gcov.html @@ -0,0 +1,332 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-06-08 22:13:12Functions: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         214 :   ~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         107 : 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         214 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60         107 :   ros::Time::waitForValid();
+      61             : 
+      62         107 :   is_initialized = true;
+      63             : 
+      64         107 :   this->common_handlers = common_handlers;
+      65             : 
+      66         107 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68         214 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75         241 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77         241 :   std::stringstream ss;
+      78         241 :   ss << "activated";
+      79             : 
+      80         241 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81         241 :   is_active = true;
+      82             : 
+      83         482 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90         237 : void NullTracker::deactivate(void) {
+      91             : 
+      92         237 :   ROS_INFO("[NullTracker]: deactivated");
+      93         237 :   is_active = false;
+      94         237 : }
+      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      136027 : 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      136027 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126        7192 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128        7192 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130        7192 :   tracker_status.active            = is_active;
+     131        7192 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133        7192 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140         295 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142         590 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144         295 :   std::stringstream ss;
+     145             : 
+     146         295 :   if (cmd->data != callbacks_enabled) {
+     147             : 
+     148          12 :     callbacks_enabled = cmd->data;
+     149             : 
+     150          12 :     ss << "callbacks " << (callbacks_enabled ? "enabled" : "disabled");
+     151             : 
+     152          12 :     ROS_DEBUG_STREAM("[NullTracker]: " << ss.str());
+     153             : 
+     154             :   } else {
+     155             : 
+     156         283 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159         295 :   res.message = ss.str();
+     160         295 :   res.success = true;
+     161             : 
+     162         885 :   return std_srvs::SetBoolResponse::ConstPtr(std::make_unique<std_srvs::SetBoolResponse>(res));
+     163             : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* //{ setReference() */
+     168             : 
+     169           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr NullTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     170           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     171             : }
+     172             : 
+     173             : //}
+     174             : 
+     175             : /* //{ setVelocityReference() */
+     176             : 
+     177           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr NullTracker::setVelocityReference([
+     178             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     179           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     180             : }
+     181             : 
+     182             : //}
+     183             : 
+     184             : /* //{ setTrajectoryReference() */
+     185             : 
+     186           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr NullTracker::setTrajectoryReference([
+     187             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     188           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     189             : }
+     190             : 
+     191             : //}
+     192             : 
+     193             : // | --------------------- other services --------------------- |
+     194             : 
+     195             : /* //{ hover() */
+     196             : 
+     197           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     198           0 :   return std_srvs::TriggerResponse::Ptr();
+     199             : }
+     200             : 
+     201             : //}
+     202             : 
+     203             : /* //{ startTrajectoryTracking() */
+     204             : 
+     205           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     206           0 :   return std_srvs::TriggerResponse::Ptr();
+     207             : }
+     208             : 
+     209             : //}
+     210             : 
+     211             : /* //{ stopTrajectoryTracking() */
+     212             : 
+     213           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     214           0 :   return std_srvs::TriggerResponse::Ptr();
+     215             : }
+     216             : 
+     217             : //}
+     218             : 
+     219             : /* //{ resumeTrajectoryTracking() */
+     220             : 
+     221           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     222           0 :   return std_srvs::TriggerResponse::Ptr();
+     223             : }
+     224             : 
+     225             : //}
+     226             : 
+     227             : /* //{ gotoTrajectoryStart() */
+     228             : 
+     229           0 : const std_srvs::TriggerResponse::ConstPtr NullTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     230           0 :   return std_srvs::TriggerResponse::Ptr();
+     231             : }
+     232             : 
+     233             : //}
+     234             : 
+     235             : /* //{ setConstraints() */
+     236             : 
+     237         375 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240         375 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248         107 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::NullTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html b/mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..2b6e65529b --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html @@ -0,0 +1,82 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
<unnamed>82.6 %355 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..f78e780529 --- /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:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
<unnamed>82.6 %355 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-detail.html b/mrs_uav_managers/src/transform_manager/index-detail.html new file mode 100644 index 0000000000..7ae4b1feab --- /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:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
<unnamed>82.6 %355 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-sort-f.html b/mrs_uav_managers/src/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..00f7fa097e --- /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:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-sort-l.html b/mrs_uav_managers/src/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..06c46808e6 --- /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:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index.html b/mrs_uav_managers/src/transform_manager/index.html new file mode 100644 index 0000000000..f4e5cf111b --- /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:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
82.6%82.6%
+ 		82.6 %355 / 43092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..56e9920d70 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::transform_manager::TransformManager::getName[abi:cxx11]() const0
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const7
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()107
mrs_uav_managers::transform_manager::TransformManager::onInit()107
mrs_uav_managers::transform_manager::TransformManager::TransformManager()107
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1737
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2708
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)78597
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)102329
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)154158
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)170182
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)226222
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)226222
+
+
+ + + +
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..8de4132621 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)78597
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2708
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()107
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)170182
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)154158
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)102329
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)226222
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)226222
mrs_uav_managers::transform_manager::TransformManager::onInit()107
mrs_uav_managers::transform_manager::TransformManager::TransformManager()107
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1737
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const7
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..abd6820bc8 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html @@ -0,0 +1,1038 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35543082.6 %
Date:2024-06-08 22:13:12Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..0547e52ddd60ed3d077211ec2b8e06dbee6515a3 GIT binary patch literal 3381 zcmV-54a)L~P) ze7~L5Uer9vdy8ZN8o-^bLz}HYk^cyg=T3pKt4M{$^5q1YU#8Q9I5Yi1w4_wWnd_VU1+$Mb}dD$wco+OhOdx;r)&p_TqFvX<2(-ZKLHE;a39aL z>y^M|f$~6>VuqhIJv?iMcgJ98A4g?bI5pVIK(>z*D9uc2lOpj#q%mX)ylM)qK#qGo zg@auLGc&220n_Om>2SlC2oFYO;m;3up(Z5^9e#-Vbhx#7ioj8kCo1PyRzQ(yN0|uZ zrb_ZPNB?m{i+fm)$Ixj0rX-jBX-dzX#6@A(;%WVUbn*mQ)kD{TGyJ5*djEE|0C&PY z^T>+_NNCPL1BtGTp2#|@z}#SMO73*Ah9eJiZ6b35>zR&0O=}f7mG9Y<(LH*e9X%BK z$aw>DP7CyKOgt0&+(i+qi7>Wf6m^`P;Mv(3bBh8|G8wVGVt*9KOHp)3XfNdo90l4b z!lnUxDW;CDLN~i9u1K0=DU$S?0?qU(5sxgULjqsv!;pBeL>P(lgDLvtRN!bI^Y6c& zHjVx8!5d#VRoG|jGNK|Ceku~_7$ z;x1(iaAJr3VAecnCz{O;ps`6y0f@YxB5MQpEfDQ6c8XtL-=RIj|NEE6(|`GXe3$kD z+FGIL3GwY2D4Kyr)~{LH_gYryE+|T`z@dla!WU0!-Oz&T*ePy)muS1>z$yr13O(up z_Ktf0z2Xd{2M9bO$6&HuP+O?bYmXdf<7s12vj@;BsBurKk1Y*<8uhVfHw)5nm5hlT z4***;V-z^rdz6DxEbzzczqp+|<7dyf(Y#aOmsAYcOx%K3lQ@kHS3wJW67KaC{z|Xq zB7KyBVjnYw3YZkwIqc94IdM3Ihpex2XHw?YKG$qW+A|o7AP49ABlS^cop`^~b^Ukb z__wZ4ool5J*10ahnad$rlOm%H2-qiZI9bOABPhilO9`Y2MM-xCL;H7Vk9r(k2^>6j z4F+v{=22_Fdhbb{Y4_0~AeqE^Fv4uR3Mf;wD#iKUUPF%gnE-_i{3W{KtU}e4K9?=^puzIR)&Go*9UU_8(=YGc~`8NKqro3Uwuh zc8aC<51800?a@gwQQ3R=P=H@7HyZZ7fVL>tt~{WRYA4k`eq-Q;ecaQIB6+QkPXfH4 zk7@&~Rov4G%+NzwSD`&(mg|CCO7WQIeW#j>|BP^clX~q>jvnEVU8*mwhz~!&?&oUY z{v3apfUX#z1k40OUOibFWU2c42(6@06wpV}bV3VlA;S>?ttQUe7UK!6-bb8kzOaw# z39a5o47{L^dMou54d7x0M(B~_0;wh6?DX^N8!Vh`+*q=e(=}tRxPFfx+G$I{MiIL+ zPyX=i`Qz@)69Yffox!f_C%Q8~;1;Sa@jK1$$dirKxWoWV5BIQUT&~kwIAg&(AW}1?NX=zQGbd= zE`SU_K9F?Lub%cCodY|l9-%J=U%_>;2DWf@(D}|!YXmN>F}pZwn&J>ydy4wJtXr3t zazR>=Dcs`DkfbyAg0gV&0+%{Wozr)dlmGz^wYoTpi z=98YytzBh1Amq{oZan_n#v@0-!K$*>W*Z{0hCbw z;p=ZDpv5WORnk@rj5$Zv{wWoxK73*c=P79JfwDHPnQ8-wrO;{huRa8lHvoEV&|CqH zNyXyzcz(WyZo-FD;cgZaR;lp7kE)K?)oBY+;yI!xh;HA+{zRo8V7R=F=@y&cm)VGFF{j$_90}U z7PhXDW+B$DD|u+Sg#Fe4JR#qchYeo%H>Y!Vzb@t^aNDq%=!5TM1~&V+|6{G@w(b@0 zdS}UP7k9{cq#Y-1Kk?>he6G?D<1a1oyptN|9NSdCD04BcE=egW122W7b>IVmC@r=| zZSz}J+@#J{3S$$7QVom53fxfAIoJ7B$d^^c3u*AuNAp3>Mb|V_5zr~`z2gsZ>JKkC z$nmb5!GpZ;bnm=_9AglNube1B9HvOCxFn#{l=t{x1S>oQ8@<8K`YYYoX(U%++_7r{fzXd@J3sLG*3k$#OcE%VMmYsskO|9YYvvA0KoKp!P zn2+k#_@DV`jk=bf)qJ!m9x)#yIRG-4%gjd$_-8(9)?3wh;4d;CI3K>nOPJ}cO8H;| zl{FF>k)+F?`A`jrWEH4g__GS;QL7`TXCDehYa!4_s9D29;nb{>`7bO&ELF%l>)%x< zY5jvYW?=tdpn~O$7ZkX0FnC=nFR4XgjDNdfERKHUB57|<-@EPos=N#V`Z zOaITr{d^zsbo?R{w~9hN?7E6`Sy_dW+rzK%{Tve~FP?vPFu!DGs+$rkKbS`vMg3l$ zPjEh@!x6B!h%Pcu`!@*a)RI9YHKHJExz>6t2?$EtMstzj+?Y73OMd zi~$71Xyh#G)^&_83ow;QLY@@tJ`YtdStZkl|C_gQS+vSE?-( z$&Xl9hfj|)&`L4AI_REnHtks~Mgf)(R11h8n*%*NnWk_0*Tw| z>zDNAIU6>w?N{Vq^`alk^^66{tDJdJZDX{D(;aLy15LpqG1ZFo^bv||=b*r?a*SEm zI?XDG2acMhSb5FdNGMPqa{vP0TYm3er@ii2T>4+qSq8o4 + + + + + + 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:787116967.3 %
Date:2024-06-08 22:13:12Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::elandSrv()1
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::callbackMinHeightCheck(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()2
mrs_uav_managers::uav_manager::UavManager::landSrv()5
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()5
mrs_uav_managers::uav_manager::UavManager::disarmSrv()7
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()9
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)17
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()20
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)22
mrs_uav_managers::uav_manager::UavManager::ungripSrv()23
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)79
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)80
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()81
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)81
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)84
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)93
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)94
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::uav_manager::UavManager::initialize()107
mrs_uav_managers::uav_manager::UavManager::preinitialize()107
mrs_uav_managers::uav_manager::UavManager::onInit()107
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)112
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)126
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)209
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)210
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)282
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)785
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)968
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)2233
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)22434
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)22502
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)57093
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)78068
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)170148
+
+
+ + + +
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..a51ce1f6cb --- /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:787116967.3 %
Date:2024-06-08 22:13:12Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_managers::uav_manager::UavManager::initialize()107
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()20
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)80
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)785
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)968
mrs_uav_managers::uav_manager::UavManager::preinitialize()107
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)22502
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)22434
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)22
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)282
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)170148
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)210
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)2233
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)57093
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)78068
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)17
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()9
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)209
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)84
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()81
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)93
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)79
mrs_uav_managers::uav_manager::UavManager::callbackMinHeightCheck(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)94
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)112
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)126
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)81
mrs_uav_managers::uav_manager::UavManager::onInit()107
mrs_uav_managers::uav_manager::UavManager::landSrv()5
mrs_uav_managers::uav_manager::UavManager::elandSrv()1
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()5
mrs_uav_managers::uav_manager::UavManager::disarmSrv()7
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()2
mrs_uav_managers::uav_manager::UavManager::ungripSrv()23
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..f6c3894100 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.html b/mrs_uav_managers/src/uav_manager.cpp.gcov.html new file mode 100644 index 0000000000..ff270c46f2 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.html @@ -0,0 +1,2765 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/uav_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - uav_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:787116967.3 %
Date:2024-06-08 22:13:12Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/uav_manager.cpp.gcov.png b/mrs_uav_managers/src/uav_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1fc0aab3e2329799624e481b2a00d73bba6552ac GIT binary patch literal 7293 zcmV-@9D?JCP)Ss~uQULn~a-G7y9%ufOdBrIRz)Vz0hO{)qdgwPMTT!-uRI{yB5 zv1-5gR;1S!$^<9?R(AbrqdA~O*W!jTTHwtv3IW9!iV<_%nh`^0ZQQ1HHC+?m1V&h7S2@P(HZM%Yp3(QE?uEgKcei0o z^6RM9W8|C2T%^Y6RlusF#}&=swt(vh*RcQT2)++Ua{mU5LlCd@B}a_J-dn$)848eW z9S{pz_E`Q4WL4LN_j-2j1=IA!G^8{=havHcgf+gN-6`CW28_roDl<-Sb4w}8@Lwv z*tUDfAeY}Lz&L2YvE%}Wam=?k4lo-*57kXTdW8O`!!>ryjBP}Kl~pJPyaK?nGDZYg zF}X6pQ3_ZcqiZ^1Y7AF&ZBGmWRBf?A%*KR<0hAgalp-ZpZ?}82La5033utkVZp15d*dY;(9=N*NyyD#~8&JTLYD} z7&dDH0!n~nROL|&ScmKaOu|m^o;KnLlwv?}yMpbm4p?hl9>4Q!n;INMK0t>9tx~AH zj5W*WVO3a!{w`kNR zsk>JQ(TQ1g#jIT$hvMSM6Af(GUWjq5EvB{k{A`zV?4%dv48MxRA+l0&v`xMR((KmYlQYgsQo=t`T5maa}SKfzi$YWB3+bcY^23BzF$j zQ{A)6l~5xF!Es#|vBlcVAi$a;y$Vng%bImb^DzF#=6Edu3>38bE&Xc)FHV*^AcP{0 z0sS_sa077Yu3-3PDX%0+#y;hDrKn*uXN12;gqlb|S{_9nBm=0Rvcy>Mkyhaw`e1tA zrQp=|fXK2;OK9_aW838xpm-PuSV59ulmhO=$5%6}-i3U0ZQ4ln?RP#Fs?9l|93yNs zz&?zNV*(2p2R^gCXJR&!6qj8?PO*Tq$H)k1*>w}pW8@?rjWH5K%w!+K8Sz+K?-Wjj z6i18)TGxge60d7sLv~JKCJ37`PT3bT2Gn6p^3{-VAc*xflK}aybz{VhQUfF)gj{NX zQx2sZWBUDR=Edh1BQ?m9otK+%8OkLbj7?5KZN4p2GtWK6YW)40Tf}Z?bj-*2On|q1<(WF!59Ky z>`KU{lLiD9cbz>Xpp+Pbk5hqnvByYKUDy>XrzC(YZW-V#1e_#YhYwxK{uk9WL%dL9 zUGDcE&>p}4z5f1rzW)FH^z`3&0Kj{-UGC31Km^>N%{u2gut6|0lN8Va+st*^Sn3jv zfQ4gJ5o53lcOBzCfL2roL2Cm}yNywCwy_e=?`nkIL-1$TDEs8)^6vWWKUq?nr%(Vm zsph(YF}clhwfBF!2Bu02KTj? zI~5Fz5Feo?paA83B#b*M2$)+^t1GI#&*}`!lZsoTTX(%APHr_qsI!qTee{AOJui#=bF{)GX4scy*YeGzr>$=F&VZVTH)fDIy3xUi1L{|yewzn>q%o`?Y zp5YkNte~_jMGOUq>Y5eo0UR%S?81n0#;KSpBmTn5!vClleyK!0G?MD1za^tF`#-WBu^Y-qX2s_Ru3ZH>Yh66+RFh$ZUPiKXD^|6{xI$N^MFEueiGNt z4=DEiZ62^KXTN~5MWa|Vlzr{wBhG|NEZ&M}YN(Y!oYjR$Tn|%@r2v#;baBc0RB99e zGlyQ?`9H5BZQ0alW+qn^Ym1{GMG2x^DWf(8g*vB38{U}yp zXV=cU583FEa`wkdE)=nUlIm<&PhyBA;4dx*_;?_^G$2LH{{VbuTJUvbLBnu|@j+mU z2-l{>_;6!u0*W!-$-zC4)Rll<#jjjR#fS=G*0s<~a%mbGr1q`ru9?L7_;Mw#a!QV zdoLfRv*UHCq1=Qdi?nqZYxU_oV64F;FiHZx@->t7jOK8T_Om9`bg@?UC3oh9io9Ng z+)=V86EnCf<9D@URyG>26C=rl?xj#W=C8p>CFHU+dLPC%;I3BRIOf#61+A}uIYVJq zVk9iX#C{gg_4@)WF-jtuDly~d@aevOevsqy6IKMn@EzO~Booi{5iHE@E-4MziIHSN z_flZj*M_CwCChu(8!@&4cU?0a$NQ%+L?&~*{TQo&(lukAjyK*jxfm(r=(;5-9C0bl zJJfug)elubaUqbr>u5MeD&A53KzFPz`hw|wi&zSsY+ABI2%571H*_>&s+^z|ro`of zd*dPiGjK%wLBvHOXYVWz4HMJKsea5{FKaPs%pStx)Lx8eklj>Qj-2!r_!Ss=z#fcM zKx=tC))r^A$CdTC^d5I^ajmY=@56KfN*!FvnAwu%U`}Q2Z!QI{Rx75id)PCR%6AGF z-%0hYUhNTyaf>56Y+ejVSIp_6LPNOBl3w0e-_k+zIL3uK56=J=vs_+gDJ6z0Ys`Dl zut)qRs(7xoWsQMBtPv+IYh;}`_xbUY%67O|NABeAakt_G=8U^{{Di~De;`=qmVtcPZ7%TUWws((%o*7nU4Wy z6EGJ=77*Mp(r+~{c_VL4FaS(c&AJ?WnY*IN-hhz@RA59B$spHzaV@R8!HX*eX9LC} z`QL?c@310JXAfK})n9taslXiqmEx9Ne9x-N{x!3g#tlXCU3NGY$I4?7(oOSpn~}CS zwA&mAm_Q-Nwv(UlcEnjf{B*=mN1Vp^HpfSb@glvp<@AsmVg~I|-ikE!kd}qGcuI)z z^pGM|`cmKw-{~QBVkCA=GmJ;|kbv3~DyjRCJ)}*?fzcw|JlCg(WP}mi>SpuaXz3vV z<9g}4)lKLjedWy|pOZ77XhflxYRa;9FVGo@9Ekf=ZcT551_t!gh@2X zK_Fe%v4|8FoDQ#km0u-l)k z!d!$7C&OF^iE`0_p2cSFoRCz5oQx;-Sw z#TwwkdOut-XFQY}B9-s-keD$p;kG<9NxnTk+X>`0g(#;o3cE?|zw;U%OF*m3l0`{_GGgB#?QPrWOh@BVTwvsTm0i_uCwwjZ$l~haF zv*KZMBROCzoY9kkw2Sw+n_`Fbz!~?jZ@!u>h(SJAxmffV<7uG!si4jTMCG}NUKha6 zXBDvaNQJ&@=2BVchv+a$YNc0Fx_q`eMV|4*4JnuQ2KD5?-w%am_}| zps?*MX2f1O(|{$-2=D-6Mz4`#j7JMp+T@!E^Ez@1TH41luj4WTM|X1x*AfF5vu&Tj zEDj?z!bqV~V-zpL8`h5|mru(MsH-sMPC$$f!>)QT3IMyt$l+FF6oen_b-29sh#CI| zM7NmPRIr_5RtJ2Sn8|2XYuxwbx$fjOQ}A04<)%s9Zyb(NhGUk`4H#{5>^Y?-7)`5Z z4-%fOHtyxL;`s2cFQ?&W#KU!e0pZ4nH%)4^D65$`*0P7y{AG7Plr8S_5b$Wu+5WkY zz^s>rd^qPUYq;xU%r$(<*S`!rS}CdB>PQI@Qr*?0lAedCf~`Y(!bMyYu2lA86iZLw z_h7_tx#bGNy;jeI<3I)PHsAdV5Xr`)?vteI?Bk6a_#uc?t@y0hEMmZhP`K_|Lh&CS zPGTOwBl(z}X~0zfaXvCqS@6-nslroz@^K*o79<~|WKvZBEk3G3O@wVkye zsHVs*DSH1WEdXExWKY$dd}LWk@{wZY$;ayQ)||c~6~KCK2|5EO!`JPeOAhH64z@Jr zSw!Vy5%Bei$Afu`LH91e4}1iWKUQ6?*Jn8D=DxX#$6wvA50D19<&hz)ckM%9W~Sr9 zofnrj*`vGDg&7(k=AC!C$Qr;{dGON%5)Xco&&o&^YXWTPdJNbmrmgFDx~??|^y2SC zqUKsGxJ5}d3n;~S3H8S@(9A6fW&CkYqEFXU#5(GSnGW<7R3ctX}Eyf~U%@%8%S zcsxBkGKvbZW0ugJXs=K$sVwR%SFQ$foj2^{%U5tCKCYj`#ibc*Lmiv9p69G<=K05R zjBe&^-uuW-uQdS$7&Tqfr#&;kw>`RX@vn@B)yj9!wOnT==}IkE8c z)&E~y)9k;0E-q2m2UuM0f4{zlcxA~LDn>>RIKsk_A>?S&M&bll#BS|=jd!c44@zPN ze+$d#w=a4F3!_h0wgg!ANK%`RKt<(D0ZpT%Qg+zl256^yM$f!4=Gq7UCF~k{>=sjq z88ie7gCbn&eE_wbYvjJ0VsE)R^FPV1O(3g2-8jHGvi}O0{6hdJEe{pTqwF;Ko*NQ( zoHEBaYXDnHDr0q=8B{Exr@D5-AknoHu!2^*T$2(L3WfE44VKHm^U>FwJ)eHvR@*W=O!3_A&y9NJl6{&L>-u9# z-KLm1%La0mx;drG0p4MvA_hJyrA5~1E3tk&Vo3~LSwL|70jN(@@00Oqa{PcLF+@4c z4>*ydq54ZwY;uP8l*B}NMP*!(m_Mi~3O+LNJ(`aM2fmDt2TJ*_0n*CjAlf{a!({}h z>G}(e;T=y6`DKeg9n%NQu;=i6Jcg5- zy9M-8XhK9ltfBlO+k&yfSE!W>XzM9Jp~pyF-7?~?VgDr6*|7K*5itcGFe?|J0cm3P z2h2-dU$&J8l(kqA!xYWN@9N#Xik=UEVvLna4$yKycp34$%otGC^(Q2{aasKH^?npo zuY|_U>){hdnIgSwmA!ELc-N*`T{CODm5WQLDPBJ-FzO|o7#|6^$Mx!A8C9Pq?TW-z zFZ}IcPctSJi<&0(V(uHINFxh2zAV`)lR|)1Q01l>OD(IVMTz3t%LxH+{U8Bk1y)Z? zv0pPID5ekyuu0ea>~vzZnQQZBjFX( zT}N0h;KPKZmA(e`mhUEvfSF6wGhYhR7J!n@+dOW|{(u0Gtl6t(te z)OAHGd|!c&yJ|g6dnfn(F#tC~lJ^xHAE~_9$;YZxmVETrwz5>8d~C-^!$0A!3^4LP z%E!I4Dg1HrZ_5IR1-I{gg*A{0irk(dr}Axl{ETc$C7<#~HAUz93Z6H?2B5N1f0~ah zD``FwtUUR+RImHb`S@f`eta1pkCBz4FE{AEFzLM9z@}wE7lZo^YC#wHlVb&48lm%! z155S5hc5RKE+LEeB!eOQ<<^-^sisrPLjx?}EXY%q7;E=G?va(k!b-hfdh1nE^(a>a ztW))8l@!)d*DF?9U2pmN>I17WVikw8{7~}M;o0weRieUoR|4X|!rK!5HtjJ~imdbI z?%d{Xov6Jlp@!kvc z&uyxUq8dRf3of=JD%^P>Rj*Y!> z@}ooJ3IJsoFYkNY$xFF^*DP3FV|WsC2ybF#HU;Q&n##SffO~yY_gqwJ{|~TuPIxZ^ zNpNgHCyx@ZLWNOeR5(DvWi155X`@0dxNXk|cg;IA32^-=0bIbyIY1FH8Ad0IrWiRR zmtG2lk=xz1jdLb4Aaxqo)_}aZsD(I6bo<5*-#qvlyZa@(s9a|7wA z0m{4n(ELn5@z~8J{rJf6cuhhZatK80hzw`GPw!mYM}}{diED*X^HBC2)_768M2sNo6o0@}+Gi80^}e9B@VeQq z@^-4K?5?P^%=z$7Mo-91>=3gB8rCkn;x96NrAkz#=?PsOP>r#62Z|QsL^$eOl1(eC zB!FU!+1oBGAhyl?om4KN94)KYUwfROl?Fbs`RI-px@cv%ZqWTDzYs*MQp613Hxg$P zilY_4>~Fp3p_)_x{)@?*{Q84HXbV$M9$s(pgkgEMD7Bm6*W6!*=q3_tt_eLVv) zM$>zkA*<=FJuSwmnK=uZqEbYvCFXm=inqf0NQL=nmR3*G+E>6?X$$?h=Y{QzF7YKT znvxiiHKVvS{XjN_S+O#yq~@etl7ViZq~?%(pOgn}#I0AGRR9%Km)eXN#8+su@WrXk zC~X!m-8{8f1LI)qAm!SuD&aEeDHnpxt!~SX(HRTd>_a@HB0(EVHTJnF_Q1LaUz>dD zo(9GpZtz38hkrptEIo|wvB0=rWY_@X2kIWAdQRPw>7MMy_EYyXFdiu~jCIeZs?XhZ z&j(hzSXHKm^3PNEliv^%xs*q24@KW!Y?r zAZ;Jx2Wqp^i@Tg&oEb)VXu!mtq7MyB`KPH*n}s8YOERdUkakDdS$nDnW4zP7*>ybd zYVfW-QLVTI + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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()86
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()86
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().286
+
+
+ + + +
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..4a9e1ce896 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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()86
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()86
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().286
+
+
+ + + +
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..bd0f0caac0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html @@ -0,0 +1,159 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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          86 :   GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) {
+      57          86 :   }
+      58             : 
+      59         172 :   ~GarminAgl(void) {
+      60         172 :   }
+      61             : 
+      62             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      63             :   bool start(void) override;
+      64             :   bool pause(void) override;
+      65             :   bool reset(void) override;
+      66             : 
+      67             :   mrs_msgs::Float64Stamped getUavAglHeight() const override;
+      68             :   std::vector<double>      getHeightCovariance() const override;
+      69             : };
+      70             : 
+      71             : }  // namespace garmin_agl
+      72             : 
+      73             : }  // namespace mrs_uav_state_estimators
+      74             : 
+      75             : #endif  // ESTIMATORS_STATE_GARMIN_AGL_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html new file mode 100644 index 0000000000..01180ec6c9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html @@ -0,0 +1,39 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1d069e474cef144181fc5cb4a7752617234f2348 GIT binary patch literal 380 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1{!VDxu^<<9$DTx4|5ZC|z|E~gq{) z-+wzRyyepwr_aj9N0zi)yIA@DruK$xCbdH!RA#*~FEu*Kt+Mr|wp)bOzeHP(-D@^_ zujRkIGNJuOnSSTr%L_6h^0%b~7kbxKyUa3OW%x$$>|Ep5oPyiLY^#!;A~i6^vZ+X|Ua&y>FQExYwM!Ofs>mTmP>_Uf+f44=fmvnFp3 VG2vV8@g5jd44$rjF6*2Ung9 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..f3aa09803d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..f89575c6e2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..a8526f9cbe --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..d23a7d9fd7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..b4c5a37f1d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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..a19f99c069 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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)198
mrs_uav_state_estimators::AltGeneric::~AltGeneric()198
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2198
+
+
+ + + +
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..27b19b95c8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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)198
mrs_uav_state_estimators::AltGeneric::~AltGeneric()198
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2198
+
+
+ + + +
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..2bcd1a03f4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html @@ -0,0 +1,245 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)112
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)>)116
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const232
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const238
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)272
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)>)290
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const504
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const580
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const642
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const1150
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1766
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1791
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1791
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3774
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3848
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3852
mrs_uav_state_estimators::Correction<2>::getRawCorrection()7806
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()7806
mrs_uav_state_estimators::Correction<1>::getRawCorrection()9729
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()9730
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10933
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10961
mrs_uav_state_estimators::Correction<2>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)10962
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)202296
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)206880
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)209988
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)210091
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)210119
mrs_uav_state_estimators::Correction<2>::isMsgComing()212163
mrs_uav_state_estimators::Correction<2>::isHealthy()212172
mrs_uav_state_estimators::Correction<2>::setR(double)212712
mrs_uav_state_estimators::Correction<2>::getStateId() const212942
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)216964
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)221664
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)292777
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)296904
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)434246
mrs_uav_state_estimators::Correction<1>::getStateId() const499685
mrs_uav_state_estimators::Correction<1>::setR(double)499688
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)503962
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)508602
mrs_uav_state_estimators::Correction<1>::isMsgComing()518206
mrs_uav_state_estimators::Correction<1>::isHealthy()518393
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)1008199
mrs_uav_state_estimators::Correction<2>::getR()1080932
mrs_uav_state_estimators::Correction<1>::getR()1507525
+
+
+ + + +
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..3f613389bf --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37172751.0 %
Date:2024-06-08 22:13:12Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1766
mrs_uav_state_estimators::Correction<1>::isMsgComing()518206
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)292777
mrs_uav_state_estimators::Correction<1>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)209988
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&)503962
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)210119
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()9729
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)1008199
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1791
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)296904
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()9730
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)272
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)210091
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()1507525
mrs_uav_state_estimators::Correction<1>::setR(double)499688
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)508602
mrs_uav_state_estimators::Correction<1>::isHealthy()518393
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)>)290
mrs_uav_state_estimators::Correction<2>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3774
mrs_uav_state_estimators::Correction<2>::isMsgComing()212163
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)202296
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)10962
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10933
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&)216964
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()7806
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)434246
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3848
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>)206880
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()7806
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)112
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10961
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()1080932
mrs_uav_state_estimators::Correction<2>::setR(double)212712
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)221664
mrs_uav_state_estimators::Correction<2>::isHealthy()212172
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)>)116
mrs_uav_state_estimators::Correction<1>::getStateId() const499685
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const642
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const1150
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1791
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const580
mrs_uav_state_estimators::Correction<2>::getStateId() const212942
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const238
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const504
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3852
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const232
+
+
+ + + +
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..7b8152bc70 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html @@ -0,0 +1,1948 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37172751.0 %
Date:2024-06-08 22:13:12Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..36c756498e1053b5ce159ae5531f351ae61075e7 GIT binary patch literal 5385 zcmV+k753_hP)|Te0YV4?lHTt3dwaj%^yUC# zYqtZM+l;1h*tR3fF^0B3AkBFW$jXPnNcUa>S}=0mR$(+ZjIpX~Mnp%`D4<)*%vJ&m zj62LNfN>9O7-P}QfOj(>0@eWQSrjqW&=~tx04p%wjexsyU5#r+*R7?)z0?(TZCeZN zU7ICl9w#eYYaOte=?L7mI}Grb$kEt)P1jb8Hn>B&HnMGnYmHil)a7DfTPJAHZ1bvqf@0)ooJv7)?zMFe=M^ zF)MtuAOW^Uz}xN^y^Z8qx3oAc&7ND$e6EnP=K(FcM|W*Aj}-_joH#f#Z+UK|Ar}u2 zQkwy4-}efLF>xK$P)*aA2D43oCLAn{CEyM;B2u?vq!t1iEACg|8ok+gK$)N>tP~1h zvK%z)0cIZa z`3L9co}BfjAYaGGHy@I~9Vk{&4h;p!SZfL+Av>_K)w<4@Z4}oP2ydg0 z<~yVrC;(f<`guTrcpG&9qs0<P0Bq3PJRpYKv??^@>ne=5j4)>Yzz#GT0aZI?Kle={)26v8eq~lJ$le-(BP4?N zg#fK_3g^nRu6c}nNCSfaCHBInDvjo58|GqiF@e>pEl7qAGXq6Ibks!oubhSSL3IJ^)rvDEiN?+w&2%YT^1n#G9G)=Y&~wV^=6Gim#u8P`cHjw4Rs z%oygQc6~BQ#@-(Ru?ec5fUD z%Lh&5vLHRx#mHl{8OV%Aj>1HvNaUZ;_AN+FRr;y zrsKz&9c(Wiay%YkVCVl2kYlwz@Z8erl-zefUo z`%yGX^O>VD5Q^q`UDrpP5NR4vQF#v~?aPk_IdRlh%KZjnDi3al1O*XBCcQgGZ38cp zc>HhEQ`E@=_FR>6BwYJgYl!8j+6uT$;nvGwDF7}J(7K;7)96M8mA2Bksz?+au z2r^H)%nRJLVj0PUF0(nApwf<>n{$IMXq?#rt&bKC{8q*R8kMKJNpvAV`%s5L)d6d= zDOA8N)Ag{GQW>&~#}J~dY`D<yJ|2GBj@?H{LpT*}z_IIU z*GHl{fCBsgYo^-!IwuaThLqW_t4pb~X1ewqnM4L@dy*MD*T!U%8DmXW$|jy?vONO6 zWP8-Hsj)rA_H=B=Zzdy8FoUEc0giNR=KUp1<5Jk%TDI8MqptLAJ6q1lzP}u%wM^&E z%=nQhVvm$kEK9Vi6i;RhDZt4ZYu;ILpSuUn&8;R=Ve9&d3;>VvGYtSH<}v`EVgPdY z@G<~0pt&7ZPm$MBl%xLyn-7V352$-_b~*09S*S^!8@;^_c)5bZmuvV4Hn7X@lYe{Z z`eBIL&44>ckPtJPcuu4db_~kbi>~3`1*8H?8kv+>hOwrdWtz>+%Q36Fph^avc+@)W zpcI9~DL_-<_W^3w?OsJe#nT2BcyshgF>!XbE<7Q!g{`#7QjA$U&*N&YAG`FMEF8@{D_1um4 z(YX(LWUkx?4`^;{ju*vP!IUEFQ+8}{-;~Cg2bSD{592)_^YWn>(&bu`z8{ENSa3Br zv$*E&1vxVA^hi8Oviwbeqg>ng6(bXtR!!pMRRmNu(wZ71jG+MhkP0?2559bQRt1|h z9vINSL=GASXz|SOaW@cM)`~ z&;iAyVLjkTd`JRH94~-#+jlOwp3QR9)A4=?!E9jmdl+!(Hd*9p+0{7zIRYkkn5uCc zBcN*1f|{vnfdILEYX&Hf_AN4?SY1qGl;ta&QlI?QfD`K4fKj*4PBSbH2qaOOU3jd8 zNwWe_nO!#{)1GU|F2Z)iEYw$GWP5_p{aqsywAsM+7%*Cv>?)?uk>b*GDEgGI@h+A) z#_s*+OWd_`QR0+WE(%ayxnAS1u3W#gAYJQ>n-$kOv-EszrMS5j0LAEt11x08J<{h1 z`Ub)*_S(RZx(&qiiqaxz%=2~x$%G$H_r1llcjy>EgMG{qN-4JIzShhmoRnx-Nn@}n zQ@kS&)Vf-1%2!xi*L4E}PC^=X&gGi2wBUhJN(3uCeuleO76KY>Sb5WEE2_Nv$BU@o zcl>-2ct2sV446j!ijyiDEziN(wURVUgliez24`-_sb7A$toi7pC?So0GhWp2ycwh1 z&!eN)lJ4%2Bzcu;X5spKlO!cESCS+cX8uu?dnZYxx zk$cb~xg>-`FQd0-r!!MdgzeYfg2_ajR44!!0O|qD450dC$vVK|ByAraN?FBqc_KHT zFpt#_^JdQD*3wvOAC)7K{ijUU6rNQk4hqjo z`xFSsJ$*6*RFigdGtBJ+BEw-)2cB#{Y5<}Q&ldLqJPQ72 zEJfh?3XyDWKYVsi36U7CiE=-(Gnxdk+~Esw_k+7Ip%LLO4`@F zZN~PlZ0H<&c!z#Ej+^jyW{x<=Ze0xX%l_5Jw;S4WNxcORf02HZNJ@ali}Xzwp8pu zaDI0N5-<#;Hw~Kl-(T@z-hPqWXU8y3o z4*=7P5x(#ou2C~nsL6t{*yN(SA68&AU8+(ia5IgE=p%92e2LKEo!@i*>fr6u(h zLt;HO1mrz%2zcE6xIUojBd9*A8a4d!gV)~fc+dt)kyoN4EQPfh)p70VeZly6z@Xmh zreXn9x?WEs&HoID-iT!bsP!uhfLwq80Xuvt#74lsuoV3R+l%2>Ernt$)>w)tOR8(4T7m<(In>yP|inKG{tJ+NF(}gfjR+hxkFXmod2~(n`*3}P9oT+KbuEq%L@c@5G_kIjjS5GES^I^-+JY>Jn z0}g;1`dg@LSK#WM!`|28c=iHD6EmiE+dmWsnDJ9u6i<>3#}+M~0qcG5#AD5K?Ou5G z4g2dm%#onxW8L$AbzFq}_PvRy(<_K-Sw7Ikpe#xRDbWM4rQ#1?0ulw102?)mZ=VWS zcEQVU<5hV?*D^x0LRONvU(bN&{SbcboY`P96Gm;aIqlLh9#g>5#{YyNw|5QZi!65E zom#7S3qsUJu|gqpYlcjg6%E9g%%U=LK|iRHtqD?VJqdSXI`XI^6^&tyfDJBb){^)~SLqk= zgQIR3FeI>#xnUqA%ZRXA+RX>R<&-)O_uyUCD)IDEpoYFaIGo4{F&YrjJfm#K2d|Lq zOc%C0;{9_Amta#o&vha*%fRbHIP^(*LL&hWGys|zs4xK3^e^A4)bMl%DzD)n&qcPB zclF1DLb!!PsbEY4R$%0isx*RoJ~E8_(QgrB{A5iWH=2hL7>GFQ0N`GeE1epm8Yq7{ z|CpBHB+$l1vuKqCEWVW60;m)M*DXjxSPhkIGfYKw;lQ^KjQJCMOO8Drc4-tN6)qx3 zmx)kq@N-5Ej|Lhak2D9R5VAFY-6=K)cHQez#9_sdfzSZGVHoCb^myeNVt7_xyKbf{ z5VEdSr7(A`_uEySsqjKkF@8qcWv8-6*4}0n(=~;=p>|a-+#OSI;f&N2>R#i6EpBE5 zP*!tuU0W9<^E&g=eM|?qW~_HBH!=7L56ReP-?uYUKD~%G8z(Dg6&b}dRf4@ z7;#Qq`nFg>3ov$yu}>740MXEdvBC#!mRwce59Pv&Rsrfn4k@4laF|gO^?rw*l~NX@ zk1+Pi301QJU^8dlxK?J(7L++gJ|Sil&~e9Dy_|Hw5VdDXPJ(MCFrR=}UdfgF_6A2y nf;%yNWsYc?+l&Bd8Uyzap0*uR4bz`&00000NkvXXu0mjfP@f&P literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html new file mode 100644 index 0000000000..2b4fd2d20f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-06-08 22:13:12Functions: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..10cbc64d0b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-06-08 22:13:12Functions: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..35f4f8a4d3 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html @@ -0,0 +1,243 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-06-08 22:13:12Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c826730ad70614286d57a68cb29d1eed954df391 GIT binary patch literal 663 zcmV;I0%-k-P)4zK6(s~1FktrePxJa#)*WXO@}bs+5PYc2AyWg@7ASK1 zmo$>`V`ShNVzi=_j5E@Ga0&E!#xAr5cGX+pFeHTm>DC^BgH2eKUSeGGUW|YR z7&{Nn{v{A*73@1M0FWYBF}}EpfuKx5)^W|@6!sV!tj=T%G@_9|!6&8Y7|lqhO`wNH z;HZh3g-;(@)7!jR#8}J%xfo%I*N{%(K31_-x+M}G^;wH+^4D%7$DuvpqLY;~ z-0$njF*!iuxSFPdXk6j0=qfLdi_%n+A*+D#!T7|JPxr`>pUGyW&0YqINimidlfp*2 zo;|&|SkS7s)fC%t+fVx3t7C + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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)112
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()112
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().2112
+
+
+ + + +
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..94b29a702e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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)112
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()112
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().2112
+
+
+ + + +
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..53c14a229e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html @@ -0,0 +1,191 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-06-08 22:13:12Functions: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         112 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70         112 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71         112 :   }
+      72             : 
+      73         224 :   ~HdgPassthrough(void) {
+      74         224 :   }
+      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..7b11a69b0d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-06-08 22:13:12Functions: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&)112
+
+
+ + + +
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..296bdac2dd --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-06-08 22:13:12Functions: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&)112
+
+
+ + + +
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..dd7b464641 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-06-08 22:13:12Functions: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         112 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23         112 :   }
+      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..195ec3c842 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-06-08 22:13:12Functions: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..5542a987b0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-06-08 22:13:12Functions: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..c9ba331bc8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-06-08 22:13:12Functions: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..199e9e0ac7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-06-08 22:13:12Functions: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..c6c16205a5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-06-08 22:13:12Functions: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..9835807c00 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-06-08 22:13:12Functions: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..74e2fc2665 --- /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:40576353.1 %
Date:2024-06-08 22:13:12Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.0%51.0%
+ 		51.0 %371 / 72766.2 %47 / 71
<unnamed>51.0 %371 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html new file mode 100644 index 0000000000..31b0b7c8ba --- /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:40576353.1 %
Date:2024-06-08 22:13:12Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.0%51.0%
+ 		51.0 %371 / 72766.2 %47 / 71
<unnamed>51.0 %371 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html new file mode 100644 index 0000000000..bad472ccd9 --- /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:40576353.1 %
Date:2024-06-08 22:13:12Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.0%51.0%
+ 		51.0 %371 / 72766.2 %47 / 71
<unnamed>51.0 %371 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html new file mode 100644 index 0000000000..2ffd3f8b8f --- /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:40576353.1 %
Date:2024-06-08 22:13:12Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.0%51.0%
+ 		51.0 %371 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html new file mode 100644 index 0000000000..49b8b5eafc --- /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:40576353.1 %
Date:2024-06-08 22:13:12Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
51.0%51.0%
+ 		51.0 %371 / 72766.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html new file mode 100644 index 0000000000..9f226b8cde --- /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:40576353.1 %
Date:2024-06-08 22:13:12Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.h +
100.0%
+ 		100.0 %6 / 6100.0 %3 / 3
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html new file mode 100644 index 0000000000..e88d171197 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-06-08 22:13:12Functions: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> ()>)112
mrs_uav_state_estimators::LatGeneric::~LatGeneric()112
mrs_uav_state_estimators::LatGeneric::~LatGeneric().2112
+
+
+ + + +
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..3106c1945f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-06-08 22:13:12Functions: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> ()>)112
mrs_uav_state_estimators::LatGeneric::~LatGeneric()112
mrs_uav_state_estimators::LatGeneric::~LatGeneric().2112
+
+
+ + + +
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..04f9f83b54 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html @@ -0,0 +1,250 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-06-08 22:13:12Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6fa70431bbed0c82bdfc8693d0379281e7228df2 GIT binary patch literal 708 zcmV;#0z3VQP)00{{R3zw{NF0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpKi;y_O zOB%{}8Z7WkVq8Vb85a;TMu+qO%z?qqs6t!d`_Ki(-X4L2*k+}t7^nPr@-bN4TCPl_ zAGp6E9d7CK7+_eqUf?z7BcRy=1apRLR>%N^9({@URod!l4E(Mq!gNnhOh#%nr`xJF z0&gcCa`_Kj2fR~XXGX)?Of0kTg4lj1u0XNaX4Yj}%I_Dq9E>oi0UM9B2aPcLmq6+; z8SRj4#@J{W09l#!j6(GZlPQ;|JTn?&hqW1v0Y)_RlK8rk90QMZNdi37H%47_7c7NB zOri%l;k@Xz1ZB76BAWEk^e%km5%UsOeFKDr$-Ws)8B`WmUuTojJrVIpXJz-)Q?=Qo z_U~}{q)@~Zrvmbe*OZrB)>9@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().2112
+
+
+ + + +
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..fd622ece3b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().2112
+
+
+ + + +
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..fee8e9a895 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-06-08 22:13:12Functions: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         112 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24         112 :   }
+      25             : 
+      26         112 :   ~LateralEstimator(void) {
+      27         112 :   }
+      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..14ab767768 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().2112
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&)112
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().2112
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&)198
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2198
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const155640
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const185319
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&) const185325
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const185328
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const185331
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const205355
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const205365
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const205379
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const323340
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const323480
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&) const323783
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const323832
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2775882
+
+
+ + + +
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..d325ed8d4d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().2112
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&)198
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2198
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&)112
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().2112
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() const205379
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const205365
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const205355
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&) const323783
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const323480
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const155640
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const323832
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const323340
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&) const185325
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const185331
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2775882
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const185328
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const185319
+
+
+ + + +
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..44c37f85b1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html @@ -0,0 +1,264 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-06-08 22:13:12Functions: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         422 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62         422 :   }
+      63             : 
+      64         422 :   ~PartialEstimator(void) {
+      65         422 :   }
+      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      714525 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103      714525 :   const states_t      states = getStates();
+     104      713770 :   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     3205736 :   for (int i = 0; i < states.size(); i++) {
+     109     2490112 :     states_vec.push_back(states(i));
+     110             :   }
+     111     1427200 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117      714014 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118      714014 :   const covariance_t  covariance = getCovarianceMatrix();
+     119      713885 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123     3204320 :   for (int i = 0; i < covariance.rows(); i++) {
+     124    12891802 :     for (int j = 0; j < covariance.cols(); j++) {
+     125    10399295 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128     1427658 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134     2931522 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135     2931522 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141      714190 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143      714190 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147     1428124 :   mrs_msgs::EstimatorOutput msg;
+     148      713318 :   msg.header.stamp    = ros::Time::now();
+     149      714633 :   msg.header.frame_id = getFrameId();
+     150      714438 :   msg.state           = getStatesAsVector();
+     151      712876 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153      713645 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160      509108 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162      509108 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166     1014010 :   mrs_msgs::Float64ArrayStamped msg;
+     167      505792 :   msg.header.stamp = stamp;
+     168     1198596 :   for (int i = 0; i < u.rows(); i++) {
+     169      688982 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172      506226 :   ph_input_.publish(msg);
+     173             : }
+     174             : /*//}*/
+     175             : 
+     176             : /*//}*/
+     177             : 
+     178             : }  // namespace mrs_uav_state_estimators
+     179             : 
+     180             : #endif  // ESTIMATORS_PARTIAL_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..53598245fe --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html @@ -0,0 +1,65 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
68.3%68.3%
+ 		68.3 %28 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html new file mode 100644 index 0000000000..8e3c9dc1bb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_excessive_tilt.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:284168.3 %
Date:2024-06-08 22:13:12Functions: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&)90
mrs_uav_state_estimators::ProcExcessiveTilt<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)153878
+
+
+ + + +
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..d9f561fee1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_excessive_tilt.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:284168.3 %
Date:2024-06-08 22:13:12Functions: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>&)153878
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&)90
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..14338f34d5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html @@ -0,0 +1,206 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_excessive_tilt.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:284168.3 %
Date:2024-06-08 22:13:12Functions: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          90 : 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          90 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      45             : 
+      46             :   // | --------------------- load parameters -------------------- |
+      47          90 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      48             : 
+      49          90 :   ph->param_loader->loadParam("orientation_topic", orientation_topic_);
+      50             :   double max_tilt;
+      51          90 :   ph->param_loader->loadParam("max_tilt", max_tilt);
+      52             : 
+      53          90 :   max_tilt = M_PI * (max_tilt / 180.0);
+      54             : 
+      55          90 :   max_tilt_sq_ = std::pow(max_tilt, 2);
+      56             : 
+      57          90 :   if (!ph->param_loader->loadedSuccessfully()) {
+      58           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+      59           0 :     ros::shutdown();
+      60             :   }
+      61             : 
+      62             :   // | -------------- initialize subscribe handlers ------------- |
+      63          90 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          90 :   shopts.nh                 = nh;
+      65          90 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          90 :   shopts.no_message_timeout = ros::Duration(1.0);
+      67          90 :   shopts.threadsafe         = true;
+      68          90 :   shopts.autostart          = true;
+      69          90 :   shopts.queue_size         = 10;
+      70          90 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          90 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch->uav_name + "/" + orientation_topic_);
+      73          90 : }
+      74             : /*//}*/
+      75             : 
+      76             : /*//{ process() */
+      77             : template <int n_measurements>
+      78      153878 : std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process([[maybe_unused]] measurement_t& measurement) {
+      79             : 
+      80      153878 :   if (!Processor<n_measurements>::enabled_) {
+      81           0 :     return {true, true};
+      82             :   }
+      83             : 
+      84      153878 :   if (!sh_orientation_.hasMsg()) {
+      85           0 :     return {false, false};
+      86             :   }
+      87             : 
+      88      153881 :   bool ok_flag     = true;
+      89      153881 :   bool should_fuse = true;
+      90             : 
+      91             :   try {
+      92      153881 :     Eigen::Matrix3d orientation_R = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion);
+      93             : 
+      94      153875 :     const double tilt = mrs_lib::geometry::angleBetween(Eigen::Vector3d(0, 0, 1), orientation_R.col(2));
+      95             : 
+      96      153877 :     const bool is_excessive_tilt = std::pow(tilt, 2) > max_tilt_sq_;
+      97             : 
+      98      153864 :     if (is_excessive_tilt) {
+      99           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: excessive tilt of %.2f deg. Not fusing correction.", Processor<n_measurements>::getPrintName().c_str(), tilt / M_PI * 180);
+     100           0 :       ok_flag     = false;
+     101           0 :       should_fuse = false;
+     102             :     }
+     103             :   }
+     104           0 :   catch (...) {
+     105           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed obtaining tilt value", Processor<n_measurements>::getPrintName().c_str());
+     106           0 :     ok_flag     = false;
+     107           0 :     should_fuse = false;
+     108             :   }
+     109      153878 :   return {ok_flag, should_fuse};
+     110             : }
+     111             : /*//}*/
+     112             : 
+     113             : /*//{ reset() */
+     114             : template <int n_measurements>
+     115           0 : void ProcExcessiveTilt<n_measurements>::reset() {
+     116             :   // no need to do anything
+     117           0 : }
+     118             : /*//}*/
+     119             : 
+     120             : }  // namespace mrs_uav_state_estimators
+     121             : 
+     122             : #endif  // PROCESSORS_PROC_EXCESSIVE_TILT_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html new file mode 100644 index 0000000000..8e05401d3b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html @@ -0,0 +1,51 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b64a252cab730e64e7e46966d4a4e6187ae9facd GIT binary patch literal 656 zcmV;B0&o3^P)2i$Eh$_a1ffbCT**)NUE|Y6jd4E=SUmi7Q$-F96_Fewyni(}s z8H0^6tmnSunwqp;B?-iQBu~`s;jK0u*OC-l;Z5Ys$lp==NYoYb-EkG;j$#=aohb^A zxVIw9f>6Q96v!N|V`^ZI1H|4neH=Q$4G>C#U=C;w|JZ<$n#o3HGK^`ZTPo(aw=>Y% zzB1#x?l9&V0|MQ>Ul@IbO5jqACP8pC?h0+-6628-4W2XVGHK_b0-on5*kxbW=QcA2t_n<dA`)n{kiFQy@)L%bU+m{=J%6!JwI^2f)b#rDD!qJ%x^-mM`xIlI q49GSm>wgvaPj~7ls(-H<<@^E?wHgS_z&(2a0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-06-08 22:13:12Functions: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&)90
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)153880
+
+
+ + + +
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..6529e76cb2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-06-08 22:13:12Functions: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>&)153880
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&)90
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..e1e74ca750 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html @@ -0,0 +1,192 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-06-08 22:13:12Functions: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          90 : 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          90 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43          90 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45          90 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46          90 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48          90 :   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          90 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55          90 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58         180 :   for (int i = 0; i < n_measurements; i++) {
+      59          90 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61          90 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      153880 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68      153880 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72      153880 :   bool ok_flag     = true;
+      73      153880 :   bool should_fuse = true;
+      74      307745 :   for (int i = 0; i < measurement.rows(); i++) {
+      75      153846 :     vec_mf_[i].add(measurement(i));
+      76      153866 :     if (vec_mf_[i].full()) {
+      77      144956 :       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        8910 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88        8910 :       ok_flag     = false;
+      89        8910 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93      153861 :   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..889a1f72bd --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-06-08 22:13:12Functions:4666.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcSaturate<1>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)4
mrs_uav_state_estimators::ProcSaturate<1>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)92
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)3854
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)155641
+
+
+ + + +
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..0da7787b10 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-06-08 22:13:12Functions: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>&)155641
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)>)92
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)3854
mrs_uav_state_estimators::ProcSaturate<2>::ProcSaturate(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&, mrs_uav_managers::estimation_manager::StateId_t, std::function<double (int, int)>)4
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html new file mode 100644 index 0000000000..5f4af983f1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html new file mode 100644 index 0000000000..7aaa9585a2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-06-08 22:13:12Functions: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          96 : 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          96 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48          96 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50          96 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51          96 :   this->enabled_ = this->start_enabled_;
+      52          96 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53          96 :   ph->param_loader->loadParam("min", saturate_min_);
+      54          96 :   ph->param_loader->loadParam("max", saturate_max_);
+      55          96 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57          96 :   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          96 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      159495 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69      159495 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73      159495 :   bool ok_flag     = true;
+      74      159495 :   bool should_fuse = true;
+      75      316264 :   for (int i = 0; i < measurement.rows(); i++) {
+      76      163334 :     const double state = fun_get_state_(state_id_, i);
+      77      163338 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79      163338 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80        6559 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83      156767 :     if (measurement(i) > state + saturate_max_) {
+      84        1034 :       const double saturated = state + saturate_max_;
+      85        1034 :       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        1034 :       measurement(i) = saturated;
+      88        1034 :       ok_flag        = false;
+      89        1034 :       should_fuse    = true;
+      90      155747 :     } 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      152928 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105      152928 :   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..f1f5b229ad --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-06-08 22:13:12Functions: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&)108
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)73069
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)206892
+
+
+ + + +
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..f2883261ad --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-06-08 22:13:12Functions: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>)73069
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)206892
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&)108
+
+
+ + + +
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..804b9e4dbb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html @@ -0,0 +1,241 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-06-08 22:13:12Functions:3837.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Processor<1>::toggle(bool)0
mrs_uav_state_estimators::Processor<2>::toggle(bool)0
mrs_uav_state_estimators::Processor<2>::Processor(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)112
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const115
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const191
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&)272
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const362
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const576
+
+
+ + + +
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..ce457883e2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-06-08 22:13:12Functions: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&)272
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&)112
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const191
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const362
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const576
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const115
+
+
+ + + +
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..b51decbfb1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-06-08 22:13:12Functions: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         384 :   Processor([[maybe_unused]] ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      28             :             const std::shared_ptr<PrivateHandlers_t>& ph)
+      29         384 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30         384 :   }  // 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         477 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52         477 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58         767 : std::string Processor<n_measurements>::getPrintName() const {
+      59         767 :   return ch_->nodelet_name + "/" + correction_name_ + "/" + name_;
+      60             : }
+      61             : /*//}*/
+      62             : 
+      63             : /*//{ toggle() */
+      64             : template <int n_measurements>
+      65           0 : void Processor<n_measurements>::toggle(bool enable) {
+      66           0 :   enabled_ = enable;
+      67           0 : }
+      68             : /*//}*/
+      69             : 
+      70             : }  // namespace mrs_uav_state_estimators
+      71             : 
+      72             : #endif  // PROCESSORS_PROCESSOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html new file mode 100644 index 0000000000..804fb09cf2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html @@ -0,0 +1,38 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()86
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&)86
mrs_uav_state_estimators::garmin_agl::GarminAgl::isConverged()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)153229
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)1586
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()86
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const154217
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..46161d9906 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html @@ -0,0 +1,318 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-06-08 22:13:12Functions: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          86 : void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          86 :   ch_ = ch;
+      17          86 :   ph_ = ph;
+      18          86 :   nh_ = nh;
+      19             : 
+      20          86 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      21             : 
+      22             :   // | --------------------- load parameters -------------------- |
+      23             : 
+      24          86 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          86 :   if (is_core_plugin_) {
+      27             : 
+      28          86 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      29          86 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          86 :   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          86 :   _update_timer_rate_       = 100;                                                                                          // TODO: parametrize
+      39          86 :   timer_update_             = nh.createTimer(ros::Rate(_update_timer_rate_), &GarminAgl::timerUpdate, this, false, false);  // not running after init
+      40          86 :   _check_health_timer_rate_ = 1;                                                                                            // TODO: parametrize
+      41          86 :   timer_check_health_       = nh.createTimer(ros::Rate(_check_health_timer_rate_), &GarminAgl::timerCheckHealth, this);
+      42             : 
+      43             :   // | --------------- subscribers initialization --------------- |
+      44         172 :   mrs_lib::SubscribeHandlerOptions shopts;
+      45          86 :   shopts.nh                 = nh;
+      46          86 :   shopts.node_name          = getPrintName();
+      47          86 :   shopts.no_message_timeout = ros::Duration(0.5);
+      48          86 :   shopts.threadsafe         = true;
+      49          86 :   shopts.autostart          = true;
+      50          86 :   shopts.queue_size         = 10;
+      51          86 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      52             : 
+      53             :   // | ---------------- publishers initialization --------------- |
+      54          86 :   ph_agl_height_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, Support::toSnakeCase(getName()) + "/agl_height", 10);
+      55          86 :   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          86 :   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          86 :   est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName(), is_core_plugin_);
+      65          86 :   est_agl_garmin_->initialize(nh, ch_, ph_);
+      66             : 
+      67          86 :   max_flight_z_ = est_agl_garmin_->getMaxFlightZ();
+      68             : 
+      69             :   // | ------------------ initialize published messages ------------------ |
+      70          86 :   agl_height_init_.header.frame_id     = ns_frame_id_;
+      71          86 :   agl_height_cov_init_.header.frame_id = ns_frame_id_;
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          86 :   if (changeState(INITIALIZED_STATE)) {
+      76          86 :     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          86 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84          86 : bool GarminAgl::start(void) {
+      85             : 
+      86             : 
+      87          86 :   if (isInState(READY_STATE)) {
+      88             : 
+      89             :     bool est_agl_garmin_start_successful;
+      90             : 
+      91          86 :     if (est_agl_garmin_->isStarted() || est_agl_garmin_->isRunning()) {
+      92           0 :       est_agl_garmin_start_successful = true;
+      93             :     } else {
+      94          86 :       est_agl_garmin_start_successful = est_agl_garmin_->start();
+      95             :     }
+      96             : 
+      97          86 :     if (est_agl_garmin_start_successful) {
+      98          86 :       timer_update_.start();
+      99          86 :       changeState(STARTED_STATE);
+     100          86 :       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      153229 : void GarminAgl::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     147             : 
+     148      153229 :   if (!isInitialized()) {
+     149           0 :     return;
+     150             :   }
+     151             : 
+     152      153229 :   const ros::Time time_now = ros::Time::now();
+     153             : 
+     154      306458 :   mrs_msgs::Float64Stamped agl_height = agl_height_init_;
+     155      153229 :   agl_height.header.stamp             = time_now;
+     156      153229 :   agl_height.value                    = est_agl_garmin_->getState(POSITION);
+     157             : 
+     158      306458 :   mrs_msgs::Float64ArrayStamped agl_height_cov;
+     159      153229 :   agl_height_cov.header.stamp = time_now;
+     160             : 
+     161      153229 :   const int n_states = 2;  // TODO this should be defined somewhere else
+     162      153229 :   agl_height_cov.values.resize(n_states * n_states);
+     163      153229 :   agl_height_cov.values.at(n_states * POSITION + POSITION) = est_agl_garmin_->getCovariance(POSITION);
+     164      153229 :   agl_height_cov.values.at(n_states * VELOCITY + VELOCITY) = est_agl_garmin_->getCovariance(VELOCITY);
+     165             : 
+     166      153229 :   mrs_lib::set_mutexed(mtx_agl_height_, agl_height, agl_height_);
+     167      153229 :   mrs_lib::set_mutexed(mtx_agl_height_cov_, agl_height_cov, agl_height_cov_);
+     168             : 
+     169      153229 :   publishAglHeight();
+     170      153229 :   publishCovariance();
+     171      153229 :   publishDiagnostics();
+     172             : }
+     173             : /*//}*/
+     174             : 
+     175             : /*//{ timerCheckHealth() */
+     176        1586 : void GarminAgl::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     177             : 
+     178        1586 :   if (!isInitialized()) {
+     179           0 :     return;
+     180             :   }
+     181             : 
+     182        1586 :   if (isInState(INITIALIZED_STATE)) {
+     183             : 
+     184          94 :     if (est_agl_garmin_->isReady()) {
+     185          86 :       changeState(READY_STATE);
+     186          86 :       ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
+     187             :     } else {
+     188           8 :       ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     189           8 :       return;
+     190             :     }
+     191             :   }
+     192             : 
+     193             : 
+     194        1578 :   if (isInState(STARTED_STATE)) {
+     195          89 :     ROS_INFO("[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     196             : 
+     197          89 :     if (est_agl_garmin_->isRunning()) {
+     198          86 :       ROS_INFO("[%s]: Subestimators converged", getPrintName().c_str());
+     199          86 :       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      154217 : mrs_msgs::Float64Stamped GarminAgl::getUavAglHeight() const {
+     219      154217 :   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          86 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::garmin_agl::GarminAgl, mrs_uav_managers::AglEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html new file mode 100644 index 0000000000..a5ae0c148f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html @@ -0,0 +1,79 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)198
mrs_uav_state_estimators::AltGeneric::isConverged()198
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)198
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)297
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)387
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const1280
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1537
mrs_uav_state_estimators::AltGeneric::start()4756
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) const155641
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const155647
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const196914
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const323512
mrs_uav_state_estimators::AltGeneric::getStates() const323756
mrs_uav_state_estimators::AltGeneric::setDt(double const&)323850
mrs_uav_state_estimators::AltGeneric::generateB()324463
mrs_uav_state_estimators::AltGeneric::generateA()324471
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)368723
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) const499316
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&)499353
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)499384
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const700265
mrs_uav_state_estimators::AltGeneric::getState(int const&) const1188716
+
+
+ + + +
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..f6ce0e2a5d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-06-08 22:13:12Functions:223366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)198
mrs_uav_state_estimators::AltGeneric::isConverged()198
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)368723
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&)499353
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)499384
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)387
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)198
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setDt(double const&)323850
mrs_uav_state_estimators::AltGeneric::start()4756
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)297
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()324471
mrs_uav_state_estimators::AltGeneric::generateB()324463
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1537
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const700265
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const196914
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const1280
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const323512
mrs_uav_state_estimators::AltGeneric::getState(int const&) const1188716
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const155647
mrs_uav_state_estimators::AltGeneric::getStates() const323756
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) const499316
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) const155641
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::AltGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..be84a0e587 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html new file mode 100644 index 0000000000..5f893c5923 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html @@ -0,0 +1,839 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-06-08 22:13:12Functions:223366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

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

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

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

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

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

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..64f48b05578bee38911c93244748b9d1c7237750 GIT binary patch literal 2310 zcmV+h3HkPkP)%-Vg0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpRGaN<3|zBMI@TE#PU-5u=>d+Ns*?Q)3)Y7#U^< za!hgbl>^pVqU$;xb4&>cRCc7tofuiptm2xK!Z6lUHdm!sd93MpiVd&RA!d@Fl~ zpE}K2G?iqGhM_VdR6pyU`r7~WGZ$ta&Ya+x7U3D$m>r9PL>W*CSM(xN-$V^S7|4Gb#!04zp`=4&Bg1sW+nIVP3r^J-9 zY3?Kt!o>(+iQOy>%?X#h2|gh#gSR6%nutr1i8iM&yHvY(Y+A{a-N%0RLQcG zo3%nVF!3o@G0Mn3AIUh*dzKW%e3fPtq&Uf0&s2_(5ARDI!uyo9Cv2@L61vDyn7f}^ zC3L7Vo@wuKmkGf^rXJzv!$ZY71jS@#uL$UOE_4+(Q#;wkcFrWR)=5m)uh zb+HE?eQw5UPPEQ{gdkw6!-rdT^bZgJB1abwA6tDQ!R-jrnPo%DaAw9cDHBu)Yu#i+ zP1Vg{;1Wu?+STS1%2Kd;E;SLPRyb>>W-K7G2}3rVzq5>*!*Q6wi%i*5KNz=xnI4+& zkDdnvytAkteH^I_bVGrtGp8^V6KOD#i zPQ5ErI1UrdE;&p8uQ@AtLTGBtJ`?82$_zayD`U6zm~sbY#`<^}W8Wwz%vq0?FmKcO zvINi3PtL`>p3v%N!_<}PYGV8ijegb!WdUZ~aUp&MQ^Y2+fsU^{$JCIk+NsrGWvw~t zd5^*Hv*QAbxSVlZhi}&|g<)oYhozu}W+6yfw>24Q3HKvpkiKhS*(JEXb#4rlYwm>T zm6-1^I37}f!)CV%(>mT=nM|M8H3Hq!78k(rUQ1!C_Bpx~E>~uLzfOp#a^pZ)9f&?8oe(YiuqUj;OT*p?7g*}dNP=(OR~U$UPG7+v z*VVhgMVaDdF7@Nevo3CjE+t6?;bBxDmpMvn;5_qWt!*k>4T=Sq9rxtG)XS(^@oHsu zTNfDAh^VeL+7K~4^{}FOy=RgxmL`S1U*Yd=C(m3Vl(kLEr(;emPljKQd*89a|Gt62KQ^z*N|Dp({1UZ^xVF6v)7AHV0|JyWrYYLWshUj#J1uRxE&+rGZUQ?gPlaTd!6Q?= z!T{ND4DD@!@aP=Zkduke-$qNpMT4ZPyAomcOe&W(C9{|op8k}E6S`vaT>uTf!X3@> zm`3`>S(!c3dNH*ygW6t_RyU7`{+jU09umnXhC6)MLn3IU?WCuNq{#U6ke(hA!|~}M zJw2pO#_js0@br*4!l#EcHu%#+dU{Ab$D0868+u4mkbHVbL-;;DqzdFR$HRI^mtXr< zPp-__Rd{ZxE|FOJpiiUqyd%`^NU1-YF0EQL_hFZf@FJZe~X>X5v z@mG$I@(p$uJp)e7l;VF|b=74|=)pPCA{Q03^EH{k=5)?)M7CsLgmCAk)KaK%2ZWa; z9}E6iDcGNZg)|I&L_6*uw60j<=(KlzMM5`t=$jCJn!3hfL3JU(qX+5@#j}DX+!DjB zBSg1h*fRv+LmAP~{Qqn=-J$oP-MrW!e9!KM`5KAO3`mmlk)B`iz*2heg2s1iES_J3 zlq=2tlWY4knS39gX$=oi#-={c%1HkGZJueA&NY|XN%R#=z2@}p46HRsXCN7O4>3V1 zgJ+{+-r=~X-VS5I)uC~K$4TH)`O|0nm@-krm?4l zW*Pp2_()}R#63!UOv~{ecvPLkD?z_y$69yFe23P&uE~EX0I{pkZT&*}_ElH~U1=58 zjo~S+;W3c$>_~AI9_Qcl5e_|^j1%u+_v3g%fn?1(iZ{B&bXqJF1L{9SG~HBD-x1KQkOjGoC&@&YWIN)KD;6lKXZA*h4`+Rsy;|B3@@*sZ1tJUydD>L!yP^4NpN0=c# zcu^z*Z}YhDP!~c|o3skmi$GuGLe+~~m!`DkzpV8jp&`NW`s3cGa g<3>XtdgrG24_bz-Sm?6hp8x;=07*qoM6N<$f*@sYQ~&?~ literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html new file mode 100644 index 0000000000..d97dbbca0c --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::HdgPassthrough::start()195
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const224
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const641
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const205329
mrs_uav_state_estimators::HdgPassthrough::getStates() const205359
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)205368
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)208694
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)209049
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)213732
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)422428
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const717815
+
+
+ + + +
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..145f414b93 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)205368
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)209049
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)213732
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>)208694
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()195
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)422428
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]() const641
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]() const224
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const205329
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const717815
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const205359
+
+
+ + + +
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..5bdd4c48e6 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html @@ -0,0 +1,391 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-06-08 22:13:12Functions: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         112 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16         112 :   ch_ = ch;
+      17         112 :   ph_ = ph;
+      18             : 
+      19         112 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21         112 :   hdg_state_      = states_t::Zero();
+      22         112 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24         112 :   innovation_ << 
+      25         112 :     0, 0;
+      26             : 
+      27             : 
+      28             :   // | --------------- param loader initialization --------------- |
+      29             : 
+      30         112 :   if (is_core_plugin_) {
+      31             : 
+      32         112 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      33         112 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      34             :   }
+      35             : 
+      36         112 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      37             : 
+      38             :   // | --------------------- load parameters -------------------- |
+      39         112 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      40             : 
+      41         112 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      42         112 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      43             : 
+      44         112 :   if (!ph->param_loader->loadedSuccessfully()) {
+      45           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      46           0 :     ros::shutdown();
+      47             :   }
+      48             : 
+      49             :   // | ------------------ timers initialization ----------------- |
+      50         112 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      51         112 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      52             : 
+      53             :   // | --------------- subscribers initialization --------------- |
+      54             :   // subscriber to odometry
+      55         224 :   mrs_lib::SubscribeHandlerOptions shopts;
+      56         112 :   shopts.nh                 = nh;
+      57         112 :   shopts.node_name          = getPrintName();
+      58         112 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      59         112 :   shopts.threadsafe         = true;
+      60         112 :   shopts.autostart          = true;
+      61         112 :   shopts.queue_size         = 10;
+      62         112 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      63             : 
+      64         224 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      65         112 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      66         224 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      67         112 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      68             : 
+      69             :   // | ---------------- publishers initialization --------------- |
+      70         112 :   if (ch_->debug_topics.output) {
+      71         112 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      72             :   }
+      73         112 :   if (ch_->debug_topics.diag) {
+      74           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+      75             :   }
+      76             : 
+      77             :   // | ------------------ finish initialization ----------------- |
+      78             : 
+      79         112 :   if (changeState(INITIALIZED_STATE)) {
+      80         112 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+      81             :   } else {
+      82           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      83             :   }
+      84         112 : }
+      85             : /*//}*/
+      86             : 
+      87             : /*//{ start() */
+      88         195 : bool HdgPassthrough::start(void) {
+      89             : 
+      90         195 :   if (isInState(READY_STATE)) {
+      91         112 :     timer_update_.start();
+      92         112 :     changeState(STARTED_STATE);
+      93         112 :     return true;
+      94             : 
+      95             :   } else {
+      96          83 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      97          83 :     return false;
+      98             :   }
+      99             : }
+     100             : /*//}*/
+     101             : 
+     102             : /*//{ pause() */
+     103           0 : bool HdgPassthrough::pause(void) {
+     104             : 
+     105           0 :   if (isInState(RUNNING_STATE)) {
+     106           0 :     changeState(STOPPED_STATE);
+     107           0 :     return true;
+     108             : 
+     109             :   } else {
+     110           0 :     return false;
+     111             :   }
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ reset() */
+     116           0 : bool HdgPassthrough::reset(void) {
+     117             : 
+     118           0 :   if (!isInitialized()) {
+     119           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     120           0 :     return false;
+     121             :   }
+     122             : 
+     123           0 :   changeState(STOPPED_STATE);
+     124             : 
+     125           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     126             : 
+     127           0 :   return true;
+     128             : }
+     129             : /*//}*/
+     130             : 
+     131             : /*//{ callbackOrientation() */
+     132      213732 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     133             : 
+     134      213732 :   if (!isInitialized()) {
+     135           6 :     return;
+     136             :   }
+     137             : 
+     138             :   double hdg;
+     139             :   try {
+     140      213718 :     hdg = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     141             :   }
+     142           0 :   catch (...) {
+     143           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     144             :   }
+     145             : 
+     146      213704 :   setState(hdg, POSITION);
+     147             : 
+     148      213690 :   if (!isError()) {
+     149      213662 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     150             :   }
+     151             : }
+     152             : /*//}*/
+     153             : 
+     154             : /*//{ callbackAngularVelocity() */
+     155      208694 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     156             : 
+     157      208694 :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     158           1 :     return;
+     159             :   }
+     160             : 
+     161             :   double hdg_rate;
+     162             :   try {
+     163      208686 :     hdg_rate = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+     164             :   }
+     165           0 :   catch (...) {
+     166           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     167             :   }
+     168             : 
+     169      208526 :   setState(hdg_rate, VELOCITY);
+     170             : }
+     171             : /*//}*/
+     172             : 
+     173             : /* timerUpdate() //{*/
+     174      205368 : void HdgPassthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     175             : 
+     176             : 
+     177      205368 :   if (!isInitialized()) {
+     178           0 :     return;
+     179             :   }
+     180             : 
+     181      205357 :   publishOutput();
+     182      205381 :   publishDiagnostics();
+     183             : }
+     184             : /*//}*/
+     185             : 
+     186             : /*//{ timerCheckHealth() */
+     187      209049 : void HdgPassthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     188             : 
+     189      209049 :   if (!isInitialized()) {
+     190           2 :     return;
+     191             :   }
+     192             : 
+     193      209048 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     194             : 
+     195         112 :     changeState(READY_STATE);
+     196         112 :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     197             :   }
+     198             : 
+     199      209048 :   if (isInState(STARTED_STATE)) {
+     200             : 
+     201         112 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     202         112 :     changeState(RUNNING_STATE);
+     203             :   }
+     204             : 
+     205      209048 :   if (sh_orientation_.hasMsg()) {
+     206      207641 :     is_orient_ready_ = true;
+     207             :   } else {
+     208        1407 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     209             :   }
+     210             : 
+     211      209048 :   if (sh_ang_vel_.hasMsg()) {
+     212      205735 :     is_ang_vel_ready_ = true;
+     213             :   } else {
+     214        3313 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity yet", getPrintName().c_str());
+     215             :   }
+     216             : }
+     217             : /*//}*/
+     218             : 
+     219             : /*//{ getState() */
+     220           0 : double HdgPassthrough::getState(const int &state_id_in, const int &axis_in) const {
+     221           0 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     222             : }
+     223             : 
+     224      717815 : double HdgPassthrough::getState(const int &state_idx_in) const {
+     225      717815 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     226             : }
+     227             : /*//}*/
+     228             : 
+     229             : /*//{ setState() */
+     230           0 : void HdgPassthrough::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     231           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     232           0 : }
+     233             : 
+     234      422428 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     235             : 
+     236      422428 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     237      422214 :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     238      422255 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     239             : 
+     240      422197 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     241      422132 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     242      422245 : }
+     243             : /*//}*/
+     244             : 
+     245             : /*//{ getStates() */
+     246      205359 : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     247      205359 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_);
+     248             : }
+     249             : /*//}*/
+     250             : 
+     251             : /*//{ setStates() */
+     252           0 : void HdgPassthrough::setStates(const states_t &states_in) {
+     253           0 :   mrs_lib::set_mutexed(mtx_hdg_state_, states_in, hdg_state_);
+     254           0 : }
+     255             : /*//}*/
+     256             : 
+     257             : /*//{ getCovariance() */
+     258           0 : double HdgPassthrough::getCovariance(const int &state_id_in, const int &axis_in) const {
+     259           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     260             : }
+     261             : 
+     262           0 : double HdgPassthrough::getCovariance(const int &state_idx_in) const {
+     263           0 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_(state_idx_in, state_idx_in));
+     264             : }
+     265             : /*//}*/
+     266             : 
+     267             : /*//{ getCovarianceMatrix() */
+     268      205329 : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     269      205329 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_);
+     270             : }
+     271             : /*//}*/
+     272             : 
+     273             : /*//{ setCovarianceMatrix() */
+     274           0 : void HdgPassthrough::setCovarianceMatrix(const covariance_t &cov_in) {
+     275           0 :   mrs_lib::set_mutexed(mtx_hdg_covariance_, cov_in, hdg_covariance_);
+     276           0 : }
+     277             : /*//}*/
+     278             : 
+     279             : /*//{ getInnovation() */
+     280           0 : double HdgPassthrough::getInnovation(const int &state_idx) const {
+     281           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_(state_idx));
+     282             : }
+     283             : 
+     284           0 : double HdgPassthrough::getInnovation(const int &state_id_in, const int &axis_in) const {
+     285           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     286             : }
+     287             : /*//}*/
+     288             : 
+     289             : /*//{ getLastValidHdg() */
+     290           0 : double HdgPassthrough::getLastValidHdg() const {
+     291           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     292             : }
+     293             : /*//}*/
+     294             : 
+     295             : /*//{ getNamespacedName() */
+     296         224 : std::string HdgPassthrough::getNamespacedName() const {
+     297         448 :   return parent_state_est_name_ + "/" + getName();
+     298             : }
+     299             : /*//}*/
+     300             : 
+     301             : /*//{ getPrintName() */
+     302         641 : std::string HdgPassthrough::getPrintName() const {
+     303        1282 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     304             : }
+     305             : /*//}*/
+     306             : 
+     307             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html new file mode 100644 index 0000000000..519a26d886 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html @@ -0,0 +1,97 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)112
mrs_uav_state_estimators::LatGeneric::isConverged()112
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)112
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)218
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)368
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)368
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const676
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const1064
mrs_uav_state_estimators::LatGeneric::start()4705
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) const7698
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const185327
mrs_uav_state_estimators::LatGeneric::setDt(double const&)185329
mrs_uav_state_estimators::LatGeneric::getStates() const185331
mrs_uav_state_estimators::LatGeneric::generateA()185656
mrs_uav_state_estimators::LatGeneric::generateB()185661
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)208761
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)212478
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&)212502
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) const212554
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const393817
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const393821
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const787630
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const787634
mrs_uav_state_estimators::LatGeneric::getState(int const&) const1591936
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const1592091
+
+
+ + + +
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..3d1ee1a022 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-06-08 22:13:12Functions:253375.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)112
mrs_uav_state_estimators::LatGeneric::isConverged()112
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)208761
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&)212502
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)212478
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)218
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)112
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setDt(double const&)185329
mrs_uav_state_estimators::LatGeneric::start()4705
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)368
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)368
mrs_uav_state_estimators::LatGeneric::generateA()185656
mrs_uav_state_estimators::LatGeneric::generateB()185661
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const1064
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const787630
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const787634
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const393817
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const393821
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const676
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const185327
mrs_uav_state_estimators::LatGeneric::getState(int const&) const1591936
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const1592091
mrs_uav_state_estimators::LatGeneric::getStates() const185331
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) const212554
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) const7698
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#2}::operator()(double) const0
mrs_uav_state_estimators::LatGeneric::generateRepredictorModels(double)::{lambda(double)#1}::operator()(double) const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html new file mode 100644 index 0000000000..20cba12156 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html new file mode 100644 index 0000000000..9522d180a0 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html @@ -0,0 +1,887 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24143755.1 %
Date:2024-06-08 22:13:12Functions:253375.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::passthrough::Passthrough::isConverged()0
mrs_uav_state_estimators::passthrough::Passthrough::setUavState(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_state_estimators::passthrough::Passthrough::pause()0
mrs_uav_state_estimators::passthrough::Passthrough::reset()0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_state_estimators::passthrough::Passthrough::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)1
mrs_uav_state_estimators::passthrough::Passthrough::start()1
mrs_uav_state_estimators::passthrough::Passthrough::timerUpdate(ros::TimerEvent const&)1036
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)1069
+
+
+ + + +
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..f7a408f70b --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9712776.4 %
Date:2024-06-08 22:13:12Functions:5955.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..606fb073ca70818dc0b3b5dba84fd6a7044d0512 GIT binary patch literal 231 zcmeAS@N?(olHy`uVBq!ia0vp^0YEIt!VDxsGPYO&DTx4|5ZC|z|E~gq^JNDtZ&EXPgU2N!e?RY*!@G!k?Ta()7BTAKis#>5!bv}um4PI*Z02~7mGQc zuCNw3Xkx0Ko_*uhD$z@81X?onZt12fEV|g{c9?_v+hwVbPc627Ikr{%)|^j!tT$gZ WJ^4%e>pP$W7(8A5T-G@yGywov30pG& literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..6d9db3d070 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-06-08 22:13:12Functions: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&)112
mrs_uav_state_estimators::StateGeneric::start()8699
mrs_uav_state_estimators::StateGeneric::updateUavState()196913
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)209142
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)209145
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()() const315806
mrs_uav_state_estimators::StateGeneric::getHeading() const315809
+
+
+ + + +
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..c2b26e0709 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-06-08 22:13:12Functions: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&)112
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&)209142
mrs_uav_state_estimators::StateGeneric::updateUavState()196913
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)209145
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()8699
mrs_uav_state_estimators::StateGeneric::getHeading() const315809
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()() const315806
+
+
+ + + +
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..54342a1027 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html @@ -0,0 +1,633 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-06-08 22:13:12Functions: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         112 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13         112 :   ch_ = ch;
+      14         112 :   ph_ = ph;
+      15             : 
+      16         224 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18         112 :   if (is_core_plugin_) {
+      19             : 
+      20         112 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21         112 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24         112 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26         112 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27         112 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28         112 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29         112 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31         112 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32         112 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36         224 :   std::string topic_orientation;
+      37         112 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38         112 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39         224 :   std::string topic_angular_velocity;
+      40         112 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41         112 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43         112 :   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         112 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51         112 :   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         112 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56         224 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57         112 :   shopts.nh                 = nh;
+      58         112 :   shopts.node_name          = getPrintName();
+      59         112 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60         112 :   shopts.threadsafe         = true;
+      61         112 :   shopts.autostart          = true;
+      62         112 :   shopts.queue_size         = 10;
+      63         112 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65         112 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66         112 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69         112 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70         112 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72         112 :   if (ch_->debug_topics.state) {
+      73         112 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75         112 :   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         112 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82         112 :   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         224 :   std::vector<double> max_altitudes;
+      88             : 
+      89         112 :   if (is_hdg_passthrough_) {
+      90         112 :     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         112 :   est_hdg_->initialize(nh, ch_, ph_);
+      95         112 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97      315918 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98         112 :   est_lat_->initialize(nh, ch_, ph_);
+      99         112 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101         112 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102         112 :   est_alt_->initialize(nh, ch_, ph_);
+     103         112 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105         112 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108         112 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109         112 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111         112 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112         112 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113         112 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115         112 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116         112 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117         112 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121         112 :   if (changeState(INITIALIZED_STATE)) {
+     122         112 :     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         112 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130        8699 : bool StateGeneric::start(void) {
+     131             : 
+     132        8699 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136        8699 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137        3994 :       est_lat_start_successful = true;
+     138             :     } else {
+     139        4705 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142        8699 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143        4029 :       est_alt_start_successful = true;
+     144             :     } else {
+     145        4670 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148        8699 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149        8504 :       timer_pub_attitude_.start();
+     150        8504 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152         195 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155        8699 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157         112 :       changeState(STARTED_STATE);
+     158         112 :       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        8587 :   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      209142 : void StateGeneric::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     209             : 
+     210             : 
+     211      209142 :   if (!isInitialized()) {
+     212       12228 :     return;
+     213             :   }
+     214             : 
+     215      416831 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     216             : 
+     217      208415 :   switch (getCurrentSmState()) {
+     218             : 
+     219           0 :     case UNINITIALIZED_STATE: {
+     220           0 :       break;
+     221             :     }
+     222        2803 :     case INITIALIZED_STATE: {
+     223             : 
+     224        2803 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     225         112 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     226         112 :           changeState(READY_STATE);
+     227         112 :           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        2691 :         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        2691 :         return;
+     235             :       }
+     236             : 
+     237         112 :       break;
+     238             :     }
+     239             : 
+     240        8661 :     case READY_STATE: {
+     241        8661 :       break;
+     242             :     }
+     243             : 
+     244         150 :     case STARTED_STATE: {
+     245             : 
+     246         150 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     247             : 
+     248         150 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     249         112 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     250         112 :         changeState(RUNNING_STATE);
+     251             :       } else {
+     252          38 :         return;
+     253             :       }
+     254         112 :       break;
+     255             :     }
+     256             : 
+     257      196802 :     case RUNNING_STATE: {
+     258      196802 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     259           0 :         changeState(ERROR_STATE);
+     260             :       }
+     261      196802 :       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      205687 :   if (!isRunning() && !isStarted()) {
+     277        8773 :     return;
+     278             :   }
+     279             : 
+     280      196914 :   updateUavState();
+     281             : 
+     282      196914 :   publishUavState();
+     283      196914 :   publishOdom();
+     284      196914 :   publishCovariance();
+     285      196913 :   publishInnovation();
+     286      196914 :   publishDiagnostics();
+     287             : }
+     288             : /*//}*/
+     289             : 
+     290             : /*//{ timerCheckHealth() */
+     291           0 : void StateGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     292             : 
+     293           0 :   if (!isInitialized()) {
+     294           0 :     return;
+     295             :   }
+     296             : 
+     297           0 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299           0 :   switch (getCurrentSmState()) {
+     300             : 
+     301           0 :     case UNINITIALIZED_STATE: {
+     302           0 :       break;
+     303             :     }
+     304           0 :     case INITIALIZED_STATE: {
+     305             : 
+     306           0 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     307           0 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     308           0 :           changeState(READY_STATE);
+     309           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     310             :         } else {
+     311           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     312           0 :           return;
+     313             :         }
+     314             :       } else {
+     315           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_hw_api_orient_.topicName().c_str());
+     316           0 :         return;
+     317             :       }
+     318             : 
+     319           0 :       break;
+     320             :     }
+     321             : 
+     322           0 :     case READY_STATE: {
+     323           0 :       break;
+     324             :     }
+     325             : 
+     326           0 :     case STARTED_STATE: {
+     327             : 
+     328           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     329             : 
+     330           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     331           0 :         changeState(ERROR_STATE);
+     332             :       }
+     333             : 
+     334           0 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     335           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     336           0 :         changeState(RUNNING_STATE);
+     337             :       } else {
+     338           0 :         return;
+     339             :       }
+     340           0 :       break;
+     341             :     }
+     342             : 
+     343           0 :     case RUNNING_STATE: {
+     344           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     345           0 :         changeState(ERROR_STATE);
+     346             :       }
+     347           0 :       break;
+     348             :     }
+     349             : 
+     350           0 :     case STOPPED_STATE: {
+     351           0 :       break;
+     352             :     }
+     353             : 
+     354           0 :     case ERROR_STATE: {
+     355           0 :       if (est_lat_->isReady() && est_alt_->isReady() && est_hdg_->isReady()) {
+     356           0 :         changeState(READY_STATE);
+     357             :       }
+     358           0 :       break;
+     359             :     }
+     360             :   }
+     361             : }
+     362             : /*//}*/
+     363             : 
+     364             : /* timerPubAttitude() //{*/
+     365      209145 : void StateGeneric::timerPubAttitude([[maybe_unused]] const ros::TimerEvent &event) {
+     366             : 
+     367      209145 :   if (!isInitialized()) {
+     368        3761 :     return;
+     369             :   }
+     370             : 
+     371      416840 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     372             : 
+     373      208420 :   if (!sh_hw_api_orient_.hasMsg()) {
+     374        1350 :     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        1350 :     return;
+     376             :   }
+     377             : 
+     378      207069 :   if (!est_hdg_->isRunning() && !isError()) {
+     379        1685 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     380        1685 :     return;
+     381             :   }
+     382             : 
+     383      205385 :   scope_timer.checkpoint("checks");
+     384             : 
+     385      205384 :   const ros::Time time_now = ros::Time::now();
+     386             : 
+     387      205385 :   geometry_msgs::QuaternionStamped att;
+     388      205385 :   att.header.stamp    = time_now;
+     389      205385 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     390             : 
+     391             :   double hdg;
+     392      205384 :   if (isError()) {
+     393           0 :     hdg = est_hdg_->getLastValidHdg();
+     394             :   } else {
+     395      205385 :     hdg = est_hdg_->getState(POSITION);
+     396             :   }
+     397             : 
+     398      205383 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     399      205382 :   if (res) {
+     400      205384 :     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      205383 :   scope_timer.checkpoint("rotate");
+     407             : 
+     408      205379 :   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      205381 :   scope_timer.checkpoint("nan check");
+     414             : 
+     415      205379 :   ph_attitude_.publish(att);
+     416      205385 :   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      196913 : void StateGeneric::updateUavState() {
+     432             : 
+     433      196913 :   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      196914 :   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      393828 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     443             : 
+     444      196913 :   const ros::Time time_now = ros::Time::now();
+     445             : 
+     446      196914 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     447      196914 :   uav_state.header.stamp       = time_now;
+     448             : 
+     449             :   // do not rotate orientation if passthrough hdg
+     450      196914 :   if (est_hdg_name_ == "hdg_passthrough") {
+     451           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     452             :   } else {
+     453             :     double hdg;
+     454      196913 :     if (isError()) {
+     455           0 :       hdg = est_hdg_->getLastValidHdg();
+     456             :     } else {
+     457      196913 :       hdg = est_hdg_->getState(POSITION);
+     458             :     }
+     459             : 
+     460      196913 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     461      196911 :     if (res) {
+     462      196913 :       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      196911 :   scope_timer.checkpoint("rotate orientation");
+     470             : 
+     471      196914 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     472             : 
+     473      196913 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     474      196912 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     475      196913 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     476             : 
+     477      196912 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     478      196912 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     479      196913 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     480             : 
+     481      196914 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     482      196914 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     483      196913 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     484             : 
+     485      196911 :   scope_timer.checkpoint("fill uav state");
+     486             : 
+     487      393825 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     488      196913 :   scope_timer.checkpoint("uav state to odom");
+     489             : 
+     490      393825 :   nav_msgs::Odometry innovation = innovation_init_;
+     491      196911 :   innovation.header.stamp       = time_now;
+     492             : 
+     493      196911 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     494      196911 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     495      196911 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     496             : 
+     497      196910 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     498             : 
+     499      196914 :   scope_timer.checkpoint("innovation");
+     500             : 
+     501      393821 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     502      196906 :   pose_covariance.header.stamp  = time_now;
+     503      196906 :   twist_covariance.header.stamp = time_now;
+     504             : 
+     505      196906 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     506      196906 :   pose_covariance.values.resize(n_states * n_states);
+     507      196908 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     508      196901 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     509      196909 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     510             : 
+     511      196903 :   twist_covariance.values.resize(n_states * n_states);
+     512      196904 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     513      196909 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     514      196909 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     515             : 
+     516      196911 :   scope_timer.checkpoint("covariance");
+     517             : 
+     518      196907 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     519      196910 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     520      196905 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     521      196913 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     522      196906 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ getHeading() */
+     527      315809 : std::optional<double> StateGeneric::getHeading() const {
+     528      315809 :   if (!est_hdg_->isRunning()) {
+     529         247 :     return {};
+     530             :   }
+     531      315562 :   return est_hdg_->getState(POSITION);
+     532             : }
+     533             : /*//}*/
+     534             : 
+     535             : /*//{ setUavState() */
+     536           0 : bool StateGeneric::setUavState([[maybe_unused]] const mrs_msgs::UavState &uav_state) {
+     537             : 
+     538           0 :   if (!isInState(STOPPED_STATE)) {
+     539           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     540           0 :     return false;
+     541             :   }
+     542             : 
+     543           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     544           0 :   return false;
+     545             : }
+     546             : /*//}*/
+     547             : 
+     548             : }  // namespace mrs_uav_state_estimators
+     549             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..200fa9f980 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html @@ -0,0 +1,158 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::joy_tracker::JoyTracker::resetStatic()0
mrs_uav_trackers::joy_tracker::JoyTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::getStatus()0
mrs_uav_trackers::joy_tracker::JoyTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()20
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
+
+
+ + + +
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..950695df96 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_tracker - joy_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-06-08 22:13:12Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()20
mrs_uav_trackers::joy_tracker::JoyTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)107
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&)375
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::joy_tracker::JoyTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::joy_tracker::JoyTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
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..f8386249f3 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html @@ -0,0 +1,588 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_tracker - joy_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-06-08 22:13:12Functions: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         107 : 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         107 :   this->common_handlers_  = common_handlers;
+     136         107 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138         107 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140         107 :   nh_ = nh;
+     141             : 
+     142         107 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154         107 :   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         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164         214 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184         107 :   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         107 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195         107 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196         107 :   shopts.nh              = nh_;
+     197         107 :   shopts.node_name       = "JoyTracker";
+     198         107 :   shopts.queue_size      = 1;
+     199         107 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201         107 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205         107 :   last_update = ros::Time(0);
+     206             : 
+     207         107 :   is_initialized_ = true;
+     208             : 
+     209         107 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211         107 :   return true;
+     212             : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* //{ activate() */
+     217             : 
+     218           0 : std::tuple<bool, std::string> JoyTracker::activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     219             : 
+     220           0 :   std::stringstream ss;
+     221             : 
+     222           0 :   if (!got_uav_state_) {
+     223             : 
+     224           0 :     ss << "odometry not set";
+     225           0 :     return std::tuple(false, ss.str());
+     226             :   }
+     227             : 
+     228           0 :   if (!sh_joystick_.hasMsg()) {
+     229             : 
+     230           0 :     ss << "missing joystick goal";
+     231           0 :     return std::tuple(false, ss.str());
+     232             :   }
+     233             : 
+     234           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     235             : 
+     236           0 :   double uav_heading = 0;
+     237             : 
+     238             :   try {
+     239           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     240             :   }
+     241           0 :   catch (...) {
+     242           0 :     ROS_ERROR_THROTTLE(1.0, "[JoyTracker]: could not calculate UAV heading");
+     243             :   }
+     244             : 
+     245             :   // initialized the heading and z from the last tracker command / odometry
+     246             :   {
+     247           0 :     std::scoped_lock lock(mutex_state_);
+     248             : 
+     249           0 :     if (last_tracker_cmd) {
+     250             : 
+     251             :       // the last command is usable
+     252           0 :       state_z_       = last_tracker_cmd->position.z;
+     253           0 :       state_heading_ = last_tracker_cmd->heading;
+     254             : 
+     255             :     } else {
+     256             : 
+     257           0 :       state_z_       = uav_state.pose.position.z;
+     258           0 :       state_heading_ = uav_heading;
+     259             : 
+     260           0 :       ROS_WARN("[JoyTracker]: the previous command is not usable for activation, using Odometry instead");
+     261             :     }
+     262             :   }
+     263             : 
+     264           0 :   is_active_ = true;
+     265             : 
+     266           0 :   ss << "activated";
+     267           0 :   ROS_INFO_STREAM("[JoyTracker]: " << ss.str());
+     268             : 
+     269           0 :   return std::tuple(true, ss.str());
+     270             : }
+     271             : 
+     272             : //}
+     273             : 
+     274             : /* //{ deactivate() */
+     275             : 
+     276          20 : void JoyTracker::deactivate(void) {
+     277             : 
+     278          20 :   is_active_ = false;
+     279             : 
+     280          20 :   ROS_INFO("[JoyTracker]: deactivated");
+     281          20 : }
+     282             : 
+     283             : //}
+     284             : 
+     285             : /* //{ resetStatic() */
+     286             : 
+     287           0 : bool JoyTracker::resetStatic(void) {
+     288             : 
+     289           0 :   return false;
+     290             : }
+     291             : 
+     292             : //}
+     293             : 
+     294             : /* //{ update() */
+     295             : 
+     296      136027 : 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      408081 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300      408081 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303      136027 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305      136027 :     uav_state_ = uav_state;
+     306             : 
+     307      136027 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310      136027 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312      136027 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315      136027 :   if (!is_active_) {
+     316      136027 :     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         295 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389         590 :   std_srvs::SetBoolResponse res;
+     390         590 :   std::stringstream         ss;
+     391             : 
+     392         295 :   if (cmd->data != callbacks_enabled_) {
+     393             : 
+     394          19 :     callbacks_enabled_ = cmd->data;
+     395             : 
+     396          19 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     397          19 :     ROS_INFO_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     398             : 
+     399             :   } else {
+     400             : 
+     401         276 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402         276 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405         295 :   res.message = ss.str();
+     406         295 :   res.success = true;
+     407             : 
+     408         590 :   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         375 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467         375 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     468             : }
+     469             : 
+     470             : //}
+     471             : 
+     472             : /* //{ setReference() */
+     473             : 
+     474           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr JoyTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     475             : 
+     476           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     477             : }
+     478             : 
+     479             : //}
+     480             : 
+     481             : /* //{ setVelocityReference() */
+     482             : 
+     483           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr JoyTracker::setVelocityReference([
+     484             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     485           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     486             : }
+     487             : 
+     488             : //}
+     489             : 
+     490             : /* //{ setTrajectoryReference() */
+     491             : 
+     492           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr JoyTracker::setTrajectoryReference([
+     493             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     494           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     495             : }
+     496             : 
+     497             : //}
+     498             : 
+     499             : }  // namespace joy_tracker
+     500             : 
+     501             : }  // namespace mrs_uav_trackers
+     502             : 
+     503             : #include <pluginlib/class_list_macros.h>
+     504         107 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::joy_tracker::JoyTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..e3fb54a601 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html @@ -0,0 +1,146 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::landoff_tracker::LandoffTracker::resetStatic()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVertical()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)5
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)8
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)20
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)45
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()61
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)146
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)179
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)219
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()673
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()673
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()2213
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4480
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()15816
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()20296
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)23266
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
+
+
+ + + +
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..d0a7f78d37 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()61
mrs_uav_trackers::landoff_tracker::LandoffTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)107
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)146
mrs_uav_trackers::landoff_tracker::LandoffTracker::resetStatic()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)5
mrs_uav_trackers::landoff_tracker::LandoffTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVertical()0
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)8
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()20296
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)20
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()15816
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4480
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()673
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)219
mrs_uav_trackers::landoff_tracker::LandoffTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()673
mrs_uav_trackers::landoff_tracker::LandoffTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)179
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)45
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()2213
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)23266
+
+
+ + + +
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..cd155ce0a3 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html @@ -0,0 +1,1645 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-06-08 22:13:12Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : 
+       8             : #include <mrs_msgs/Vec1.h>
+       9             : #include <mrs_msgs/UavState.h>
+      10             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      11             : 
+      12             : #include <mrs_lib/profiler.h>
+      13             : #include <mrs_lib/mutex.h>
+      14             : #include <mrs_lib/attitude_converter.h>
+      15             : #include <mrs_lib/utils.h>
+      16             : #include <mrs_lib/geometry/cyclic.h>
+      17             : #include <mrs_lib/geometry/misc.h>
+      18             : 
+      19             : //}
+      20             : 
+      21             : /* defines //{ */
+      22             : 
+      23             : #define STOP_THR 1e-3
+      24             : 
+      25             : //}
+      26             : 
+      27             : /* using //{ */
+      28             : 
+      29             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      30             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      31             : 
+      32             : using radians  = mrs_lib::geometry::radians;
+      33             : using sradians = mrs_lib::geometry::sradians;
+      34             : 
+      35             : //}
+      36             : 
+      37             : namespace mrs_uav_trackers
+      38             : {
+      39             : 
+      40             : namespace landoff_tracker
+      41             : {
+      42             : 
+      43             : /* //{ class LandoffTracker */
+      44             : 
+      45             : // state machine
+      46             : typedef enum
+      47             : {
+      48             : 
+      49             :   IDLE_STATE,
+      50             :   LANDED_STATE,
+      51             :   STOP_MOTION_STATE,
+      52             :   HOVER_STATE,
+      53             :   ACCELERATING_STATE,
+      54             :   DECELERATING_STATE,
+      55             :   STOPPING_STATE,
+      56             : 
+      57             : } States_t;
+      58             : 
+      59             : const std::array<const char*, 7> state_names = {
+      60             : 
+      61             :     "IDLING", "LANDED", "STOPPING_MOTION", "HOVERING", "ACCELERATING", "DECELERATING", "STOPPING"};
+      62             : 
+      63             : class LandoffTracker : public mrs_uav_managers::Tracker {
+      64             : public:
+      65             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      66             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      67             : 
+      68             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd);
+      69             :   void                          deactivate(void);
+      70             :   bool                          resetStatic(void);
+      71             : 
+      72             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState& uav_state, const mrs_uav_managers::Controller::ControlOutput& last_control_output);
+      73             :   const mrs_msgs::TrackerStatus             getStatus();
+      74             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd);
+      75             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      76             : 
+      77             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd);
+      78             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd);
+      79             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd);
+      80             : 
+      81             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      82             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      83             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      84             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      85             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      86             : 
+      87             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+      88             : 
+      89             : private:
+      90             :   bool callbacks_enabled_ = true;
+      91             : 
+      92             :   mrs_uav_managers::Controller::ControlOutput last_control_output_;
+      93             :   std::mutex                                  mutex_last_control_output_;
+      94             : 
+      95             :   ros::NodeHandle nh_;
+      96             :   std::string     _uav_name_;
+      97             : 
+      98             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      99             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+     100             : 
+     101             :   // main timer
+     102             :   void       timerMain(const ros::TimerEvent& event);
+     103             :   ros::Timer timer_main_;
+     104             :   std::mutex mutex_main_timer_;
+     105             : 
+     106             :   // | ------------------------ 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         107 : 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         107 :   this->common_handlers_  = common_handlers;
+     215         107 :   this->private_handlers_ = private_handlers;
+     216             : 
+     217         107 :   _uav_name_ = common_handlers->uav_name;
+     218             : 
+     219         107 :   nh_ = nh;
+     220             : 
+     221         107 :   ros::Time::waitForValid();
+     222             : 
+     223             :   // --------------------------------------------------------------
+     224             :   // |                     loading parameters                     |
+     225             :   // --------------------------------------------------------------
+     226             : 
+     227             :   // | ---------- loading params using the parent's nh ---------- |
+     228             : 
+     229         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     230             : 
+     231         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     232             : 
+     233         107 :   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         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/landoff_tracker.yaml");
+     241         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/landoff_tracker.yaml");
+     242             : 
+     243         214 :   const std::string yaml_prefix = "mrs_uav_trackers/landoff_tracker/";
+     244             : 
+     245         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_speed", _horizontal_speed_);
+     246         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_acceleration", _horizontal_acceleration_);
+     247             : 
+     248         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     249         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_acceleration", _vertical_acceleration_);
+     250             : 
+     251         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_speed", _takeoff_speed_);
+     252         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_acceleration", _takeoff_acceleration_);
+     253             : 
+     254         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_speed", _landing_speed_);
+     255         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_acceleration", _landing_acceleration_);
+     256             : 
+     257         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_speed", _elanding_speed_);
+     258         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_acceleration", _elanding_acceleration_);
+     259             : 
+     260         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     261         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_gain", _heading_gain_);
+     262             : 
+     263         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "main_timer_rate", _main_timer_rate_);
+     264             : 
+     265         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "landing_reference", _landing_reference_);
+     266             : 
+     267         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_position_difference", _max_position_difference_);
+     268             : 
+     269         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains", _takeoff_disable_lateral_gains_);
+     270         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains_z", _takeoff_disable_lateral_gains_z_);
+     271             : 
+     272         107 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     273           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     274           0 :     return false;
+     275             :   }
+     276             : 
+     277         107 :   _tracker_dt_ = 1.0 / double(_main_timer_rate_);
+     278             : 
+     279         107 :   ROS_INFO("[LandoffTracker]: tracker_dt: %f", _tracker_dt_);
+     280             : 
+     281         107 :   state_x_       = 0;
+     282         107 :   state_y_       = 0;
+     283         107 :   state_z_       = 0;
+     284         107 :   state_heading_ = 0;
+     285             : 
+     286         107 :   speed_x_       = 0;
+     287         107 :   speed_y_       = 0;
+     288         107 :   speed_heading_ = 0;
+     289             : 
+     290         107 :   current_horizontal_speed_ = 0;
+     291         107 :   current_vertical_speed_   = 0;
+     292             : 
+     293         107 :   current_horizontal_acceleration_ = 0;
+     294         107 :   current_vertical_acceleration_   = 0;
+     295             : 
+     296         107 :   current_vertical_direction_ = 0;
+     297             : 
+     298         107 :   current_state_vertical_  = LANDED_STATE;
+     299         107 :   previous_state_vertical_ = LANDED_STATE;
+     300             : 
+     301         107 :   current_state_horizontal_  = LANDED_STATE;
+     302         107 :   previous_state_horizontal_ = LANDED_STATE;
+     303             : 
+     304             :   // | ------------------------ profiler ------------------------ |
+     305             : 
+     306         107 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "LandoffTracker", _profiler_enabled_);
+     307             : 
+     308             :   // | ------------------------ services ------------------------ |
+     309             : 
+     310         107 :   service_takeoff_ = nh_.advertiseService("takeoff", &LandoffTracker::callbackTakeoff, this);
+     311         107 :   service_land_    = nh_.advertiseService("land", &LandoffTracker::callbackLand, this);
+     312         107 :   service_eland_   = nh_.advertiseService("eland", &LandoffTracker::callbackELand, this);
+     313             : 
+     314             :   // | ------------------------- timers ------------------------- |
+     315             : 
+     316         107 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &LandoffTracker::timerMain, this, false, false);
+     317             : 
+     318             :   // | ----------------------- finish init ---------------------- |
+     319             : 
+     320         107 :   is_initialized_ = true;
+     321             : 
+     322         107 :   ROS_INFO("[LandoffTracker]: initialized");
+     323             : 
+     324         107 :   return true;
+     325             : }
+     326             : 
+     327             : //}
+     328             : 
+     329             : /* //{ activate() */
+     330             : 
+     331          45 : std::tuple<bool, std::string> LandoffTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     332             : 
+     333          90 :   std::stringstream ss;
+     334             : 
+     335          45 :   if (!got_uav_state_) {
+     336             : 
+     337           0 :     ss << "odometry not set";
+     338           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+     339           0 :     return std::tuple(false, ss.str());
+     340             :   }
+     341             : 
+     342             :   // copy member variables
+     343          90 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     344             : 
+     345             :   double uav_heading;
+     346             : 
+     347             :   try {
+     348          45 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     349             :   }
+     350           0 :   catch (...) {
+     351             : 
+     352           0 :     ss << "could not initialize the UAV heading";
+     353           0 :     ROS_ERROR_STREAM("[LandoffTracker]: " << ss.str());
+     354           0 :     return std::tuple(false, ss.str());
+     355             :   }
+     356             : 
+     357             :   // --------------------------------------------------------------
+     358             :   // |                      initial condition                     |
+     359             :   // --------------------------------------------------------------
+     360             : 
+     361             :   {
+     362          90 :     std::scoped_lock lock(mutex_goal_);
+     363             : 
+     364             :     // the last command is usable
+     365          45 :     state_x_       = uav_state.pose.position.x;
+     366          45 :     state_y_       = uav_state.pose.position.y;
+     367          45 :     state_z_       = uav_state.pose.position.z;
+     368          45 :     state_heading_ = uav_heading;
+     369             : 
+     370          45 :     speed_x_         = uav_state.velocity.linear.x;
+     371          45 :     speed_y_         = uav_state.velocity.linear.y;
+     372          45 :     current_heading_ = atan2(speed_y_, speed_x_);
+     373             : 
+     374          45 :     current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     375             : 
+     376          45 :     current_vertical_speed_     = fabs(uav_state.velocity.linear.z);
+     377          45 :     current_vertical_direction_ = uav_state.velocity.linear.z > 0 ? +1 : -1;
+     378             : 
+     379          45 :     current_horizontal_acceleration_ = 0;
+     380          45 :     current_vertical_acceleration_   = 0;
+     381             : 
+     382          45 :     goal_heading_ = uav_heading;
+     383             : 
+     384          45 :     ROS_INFO("[LandoffTracker]: initial condition: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", state_x_, state_y_, state_z_, state_heading_);
+     385             :   }
+     386             : 
+     387             :   // --------------------------------------------------------------
+     388             :   // |          horizontal initial conditions prediction          |
+     389             :   // --------------------------------------------------------------
+     390             : 
+     391             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     392             : 
+     393             :   {
+     394          45 :     std::scoped_lock lock(mutex_state_);
+     395             : 
+     396          45 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     397          45 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     398          45 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     399          45 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     400             :   }
+     401             : 
+     402             :   // --------------------------------------------------------------
+     403             :   // |           vertical initial conditions prediction           |
+     404             :   // --------------------------------------------------------------
+     405             : 
+     406             :   double vertical_t_stop, vertical_stop_dist;
+     407             : 
+     408             :   {
+     409          45 :     std::scoped_lock lock(mutex_state_);
+     410             : 
+     411          45 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     412          45 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     413             :   }
+     414             : 
+     415             :   // --------------------------------------------------------------
+     416             :   // |               heading initial condition prediction             |
+     417             :   // --------------------------------------------------------------
+     418             : 
+     419             :   {
+     420          45 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     421             : 
+     422          45 :     goal_x_ = state_x_ + stop_dist_x;
+     423          45 :     goal_y_ = state_y_ + stop_dist_y;
+     424          45 :     goal_z_ = state_z_ + vertical_stop_dist;
+     425             :   }
+     426             : 
+     427          45 :   landing_        = false;
+     428          45 :   taking_off_     = false;
+     429          45 :   is_active_      = true;
+     430          45 :   have_goal_      = false;
+     431          45 :   cause_failsafe_ = false;
+     432             : 
+     433          45 :   timer_main_.start();
+     434             : 
+     435             :   {
+     436          90 :     std::scoped_lock lock(mutex_goal_);
+     437             : 
+     438          45 :     ROS_INFO("[LandoffTracker]: stopping goal: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     439             :   }
+     440             : 
+     441          45 :   changeState(STOP_MOTION_STATE);
+     442             : 
+     443          45 :   ss << "activated";
+     444          45 :   ROS_INFO_STREAM("[LandoffTracker]: " << ss.str());
+     445             : 
+     446          45 :   return std::tuple(true, ss.str());
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : /* //{ deactivate() */
+     452             : 
+     453          61 : void LandoffTracker::deactivate(void) {
+     454             : 
+     455          61 :   is_active_                = false;
+     456          61 :   landing_                  = false;
+     457          61 :   taking_off_               = false;
+     458          61 :   current_state_vertical_   = IDLE_STATE;
+     459          61 :   current_state_horizontal_ = IDLE_STATE;
+     460             : 
+     461          61 :   timer_main_.stop();
+     462             : 
+     463          61 :   ROS_INFO("[LandoffTracker]: deactivated");
+     464          61 : }
+     465             : 
+     466             : //}
+     467             : 
+     468             : /* //{ resetStatic() */
+     469             : 
+     470           0 : bool LandoffTracker::resetStatic(void) {
+     471             : 
+     472           0 :   return false;
+     473             : }
+     474             : 
+     475             : //}
+     476             : 
+     477             : /* //{ update() */
+     478             : 
+     479      136027 : 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      408081 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     483      408081 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     484             : 
+     485             :   {
+     486      136027 :     std::scoped_lock lock(mutex_uav_state_);
+     487             : 
+     488      136027 :     uav_state_ = uav_state;
+     489             : 
+     490      136027 :     got_uav_state_ = true;
+     491             :   }
+     492             : 
+     493             :   // up to this part the update() method is evaluated even when the tracker is not active
+     494      136027 :   if (!is_active_ || cause_failsafe_) {
+     495      118784 :     return {};
+     496             :   }
+     497             : 
+     498       17243 :   position_output_.header.stamp    = ros::Time::now();
+     499       17243 :   position_output_.header.frame_id = uav_state_.header.frame_id;
+     500             : 
+     501             :   {
+     502       17243 :     std::scoped_lock lock(mutex_state_);
+     503             : 
+     504       17243 :     position_output_.position.x = state_x_;
+     505       17243 :     position_output_.position.y = state_y_;
+     506       17243 :     position_output_.position.z = state_z_;
+     507       17243 :     position_output_.heading    = state_heading_;
+     508             : 
+     509       17243 :     position_output_.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     510       17243 :     position_output_.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     511       17243 :     position_output_.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     512       17243 :     position_output_.heading_rate = speed_heading_;
+     513             : 
+     514       17243 :     position_output_.use_position_vertical   = 1;
+     515       17243 :     position_output_.use_position_horizontal = 1;
+     516       17243 :     position_output_.use_heading             = 1;
+     517       17243 :     position_output_.use_heading_rate        = 1;
+     518       17243 :     position_output_.use_velocity_vertical   = 1;
+     519       17243 :     position_output_.use_velocity_horizontal = 1;
+     520             :   }
+     521             : 
+     522             :   {
+     523       34486 :     std::scoped_lock lock(mutex_last_control_output_);
+     524             : 
+     525       17243 :     last_control_output_ = last_control_output;
+     526             :   }
+     527             : 
+     528       17243 :   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       17243 :     position_output_.disable_position_gains = false;
+     532             :   }
+     533             : 
+     534       17243 :   if (taking_off_) {
+     535        8538 :     position_output_.disable_antiwindups = true;
+     536             :   } else {
+     537        8705 :     position_output_.disable_antiwindups = false;
+     538             :   }
+     539             : 
+     540       17243 :   return {position_output_};
+     541             : }
+     542             : 
+     543             : //}
+     544             : 
+     545             : /* //{ getStatus() */
+     546             : 
+     547        2213 : const mrs_msgs::TrackerStatus LandoffTracker::getStatus() {
+     548             : 
+     549        2213 :   mrs_msgs::TrackerStatus tracker_status;
+     550             : 
+     551        2213 :   tracker_status.active            = is_active_;
+     552        2213 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     553             : 
+     554        2213 :   const bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
+     555        2213 :   const bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     556             : 
+     557        2213 :   if (idling)
+     558             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+     559        2213 :   else if (taking_off_)
+     560             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_TAKEOFF;
+     561        2213 :   else if (hovering)
+     562             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_HOVER;
+     563             :   else if (landing_)
+     564             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_LAND;
+     565             : 
+     566             :   tracker_status.have_goal = landing_ || taking_off_ || !(hovering || idling);
+     567             : 
+     568         295 :   tracker_status.tracking_trajectory = false;
+     569             : 
+     570         590 :   return tracker_status;
+     571         590 : }
+     572             : 
+     573         295 : //}
+     574             : 
+     575          19 : /* //{ enableCallbacks() */
+     576             : 
+     577          19 : const std_srvs::SetBoolResponse::ConstPtr LandoffTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+     578          19 : 
+     579             :   std_srvs::SetBoolResponse res;
+     580             :   std::stringstream         ss;
+     581             : 
+     582         276 :   if (cmd->data != callbacks_enabled_) {
+     583         276 : 
+     584             :     callbacks_enabled_ = cmd->data;
+     585             : 
+     586         295 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     587         295 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     588             : 
+     589         590 :   } else {
+     590             : 
+     591             :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     592             :     ROS_WARN_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     593             :   }
+     594             : 
+     595             :   res.message = ss.str();
+     596           0 :   res.success = true;
+     597             : 
+     598           0 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     599             : }
+     600           0 : 
+     601             : //}
+     602           0 : 
+     603           0 : /* switchOdometrySource() //{ */
+     604           0 : 
+     605             : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState& new_uav_state) {
+     606             : 
+     607           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     608             : 
+     609           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     610           0 : 
+     611           0 :   double old_heading  = 0;
+     612             :   double new_heading  = 0;
+     613             :   bool   got_headings = true;
+     614             : 
+     615           0 :   try {
+     616             :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     617           0 :   }
+     618           0 :   catch (...) {
+     619           0 :     ROS_ERROR_THROTTLE(1.0, "[LandoffTracker]: could not calculate the old UAV heading");
+     620             :     got_headings = false;
+     621             :   }
+     622           0 : 
+     623             :   try {
+     624           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     625           0 :   }
+     626           0 :   catch (...) {
+     627             :     ROS_ERROR_THROTTLE(1.0, "[LandoffTracker]: could not calculate the new UAV heading");
+     628           0 :     got_headings = false;
+     629             :   }
+     630             : 
+     631             :   std_srvs::TriggerResponse res;
+     632             : 
+     633           0 :   if (!got_headings) {
+     634           0 :     res.message = "could not calculate the heading difference";
+     635           0 :     res.success = false;
+     636           0 : 
+     637             :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     638           0 :   }
+     639           0 : 
+     640           0 :   // | --------- recalculate the goal to new coordinates -------- |
+     641           0 : 
+     642             :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     643             :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     644             :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     645           0 :   double dheading = new_heading - old_heading;
+     646           0 : 
+     647           0 :   goal_x_ += dx;
+     648           0 :   goal_y_ += dy;
+     649             :   goal_z_ += dz;
+     650           0 :   goal_heading_ += dheading;
+     651             : 
+     652           0 :   // | -------------------- update the state -------------------- |
+     653           0 : 
+     654             :   state_x_ += dx;
+     655           0 :   state_y_ += dy;
+     656             :   state_z_ += dz;
+     657             :   state_heading_ += dheading;
+     658             : 
+     659             :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     660             : 
+     661           0 :   res.message = "odometry source switched";
+     662             :   res.success = true;
+     663           0 : 
+     664             :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     665             : }
+     666           0 : 
+     667             : //}
+     668           0 : 
+     669           0 : /* //{ hover() */
+     670             : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     671           0 : 
+     672             :   std::scoped_lock lock(mutex_main_timer_);
+     673             : 
+     674             :   // copy member variables
+     675             :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     676             : 
+     677           0 :   auto [current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] =
+     678             :       mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+     679           0 : 
+     680           0 :   std_srvs::TriggerResponse res;
+     681           0 : 
+     682             :   // --------------------------------------------------------------
+     683             :   // |          horizontal initial conditions prediction          |
+     684             :   // --------------------------------------------------------------
+     685             :   {
+     686           0 :     std::scoped_lock lock(mutex_state_);
+     687           0 : 
+     688           0 :     current_horizontal_speed_ = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2));
+     689           0 :     current_vertical_speed_   = uav_state.velocity.linear.z;
+     690             :     current_heading_          = atan2(uav_state.velocity.linear.y, uav_state.velocity.linear.x);
+     691             :   }
+     692             : 
+     693             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     694             : 
+     695           0 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     696           0 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     697             :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     698             :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     699             : 
+     700             :   // --------------------------------------------------------------
+     701             :   // |           vertical initial conditions prediction           |
+     702             :   // --------------------------------------------------------------
+     703           0 : 
+     704             :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+     705           0 :   double vertical_stop_dist = current_vertical_direction * (vertical_t_stop * current_vertical_speed) / 2.0;
+     706           0 : 
+     707           0 :   // --------------------------------------------------------------
+     708             :   // |                        set the goal                        |
+     709             :   // --------------------------------------------------------------
+     710           0 : 
+     711           0 :   {
+     712             :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+     713           0 : 
+     714             :     goal_x_ = state_x_ + stop_dist_x;
+     715           0 :     goal_y_ = state_y_ + stop_dist_y;
+     716             :     goal_z_ = state_z_ + vertical_stop_dist;
+     717             :   }
+     718             : 
+     719             :   res.message = "hover initiated";
+     720             :   res.success = true;
+     721             : 
+     722           0 :   changeState(STOP_MOTION_STATE);
+     723           0 : 
+     724             :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     725             : }
+     726             : 
+     727             : //}
+     728             : 
+     729             : /* //{ startTrajectoryTracking() */
+     730           0 : 
+     731           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     732             :   return std_srvs::TriggerResponse::Ptr();
+     733             : }
+     734             : 
+     735             : //}
+     736             : 
+     737             : /* //{ stopTrajectoryTracking() */
+     738           0 : 
+     739           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     740             :   return std_srvs::TriggerResponse::Ptr();
+     741             : }
+     742             : 
+     743             : //}
+     744             : 
+     745             : /* //{ resumeTrajectoryTracking() */
+     746           0 : 
+     747           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     748             :   return std_srvs::TriggerResponse::Ptr();
+     749             : }
+     750             : 
+     751             : //}
+     752             : 
+     753             : /* //{ gotoTrajectoryStart() */
+     754         375 : 
+     755             : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     756             :   return std_srvs::TriggerResponse::Ptr();
+     757             : }
+     758         375 : 
+     759             : //}
+     760         375 : 
+     761             : /* //{ setConstraints() */
+     762         750 : 
+     763         375 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LandoffTracker::setConstraints([
+     764         375 :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd) {
+     765             : 
+     766         750 : 
+     767             :   mrs_lib::set_mutexed(mutex_constraints_, cmd->constraints, constraints_);
+     768             : 
+     769             :   ROS_INFO("[LandoffTracker]: updating constraints");
+     770             : 
+     771             :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     772             :   res.success = true;
+     773           0 :   res.message = "constraints updated";
+     774             : 
+     775           0 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     776             : }
+     777             : 
+     778             : //}
+     779             : 
+     780             : /* //{ setReference() */
+     781             : 
+     782           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LandoffTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+     783             : 
+     784           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     785             : }
+     786             : 
+     787             : //}
+     788             : 
+     789             : /* //{ setVelocityReference() */
+     790             : 
+     791           6 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LandoffTracker::setVelocityReference([
+     792             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+     793           6 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     794             : }
+     795             : 
+     796             : //}
+     797             : 
+     798             : /* //{ setTrajectoryReference() */
+     799             : 
+     800             : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LandoffTracker::setTrajectoryReference([
+     801             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+     802         179 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     803             : }
+     804         179 : 
+     805         179 : //}
+     806             : 
+     807         179 : // | ----------------- state machine routines ----------------- |
+     808             : 
+     809          57 : /* //{ changeStateHorizontal() */
+     810             : 
+     811         114 : void LandoffTracker::changeStateHorizontal(States_t new_state) {
+     812          57 : 
+     813             :   previous_state_horizontal_ = current_state_horizontal_;
+     814          57 :   current_state_horizontal_  = new_state;
+     815             : 
+     816             :   switch (current_state_horizontal_) {
+     817         122 : 
+     818             :     case STOPPING_STATE: {
+     819         122 : 
+     820             :       std::scoped_lock lock(mutex_state_);
+     821             :       current_horizontal_speed_ = 0;
+     822             : 
+     823         179 :       break;
+     824         179 :     };
+     825             : 
+     826             :     default: {
+     827             : 
+     828             :       break;
+     829             :     }
+     830         219 :   }
+     831             : 
+     832         219 :   ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names.at(previous_state_horizontal_), state_names.at(current_state_horizontal_));
+     833         219 : }
+     834             : 
+     835         219 : //}
+     836             : 
+     837          44 : /* //{ changeStateVertical() */
+     838          44 : 
+     839          44 : void LandoffTracker::changeStateVertical(States_t new_state) {
+     840             : 
+     841             :   previous_state_vertical_ = current_state_vertical_;
+     842         175 :   current_state_vertical_  = new_state;
+     843         175 : 
+     844             :   switch (current_state_vertical_) {
+     845             : 
+     846             :     case HOVER_STATE: {
+     847         219 :       taking_off_ = false;
+     848         219 :       break;
+     849             :     }
+     850             : 
+     851             :     default: {
+     852             :       break;
+     853             :     }
+     854         146 :   }
+     855             : 
+     856         146 :   ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names.at(previous_state_vertical_), state_names.at(current_state_vertical_));
+     857         146 : }
+     858         146 : 
+     859             : //}
+     860             : 
+     861             : /* //{ changeState() */
+     862             : 
+     863             : void LandoffTracker::changeState(States_t new_state) {
+     864             : 
+     865             :   changeStateVertical(new_state);
+     866         673 :   changeStateHorizontal(new_state);
+     867             : }
+     868             : 
+     869        1346 : //}
+     870             : 
+     871         673 : // | --------------------- motion routines -------------------- |
+     872             : 
+     873         673 : /* //{ stopHorizontalMotion() */
+     874         360 : 
+     875         360 : void LandoffTracker::stopHorizontalMotion(void) {
+     876             : 
+     877         313 :   {
+     878             :     std::scoped_lock lock(mutex_state_);
+     879             : 
+     880         673 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     881             : 
+     882             :     if (current_horizontal_speed_ < 0) {
+     883             :       current_horizontal_speed_        = 0;
+     884             :       current_horizontal_acceleration_ = 0;
+     885             :     } else {
+     886         673 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     887             :     }
+     888             :   }
+     889        1346 : }
+     890             : 
+     891         673 : //}
+     892             : 
+     893         673 : /* //{ stopVerticalMotion() */
+     894          52 : 
+     895          52 : void LandoffTracker::stopVerticalMotion(void) {
+     896             : 
+     897         621 :   {
+     898             :     std::scoped_lock lock(mutex_state_);
+     899             : 
+     900         673 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     901             : 
+     902             :     if (current_vertical_speed_ < 0) {
+     903             :       current_vertical_speed_        = 0;
+     904             :       current_vertical_acceleration_ = 0;
+     905             :     } else {
+     906       15816 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     907             :     }
+     908             :   }
+     909       15816 : }
+     910       15816 : 
+     911       15816 : //}
+     912             : 
+     913             : /* //{ accelerateVertical() */
+     914             : 
+     915             : void LandoffTracker::accelerateVertical(void) {
+     916       15816 : 
+     917             :   // copy member variables
+     918        4480 :   auto [current_vertical_speed, state_z] = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_, state_z_);
+     919        4480 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     920             :   auto constraints                       = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     921        4480 : 
+     922           0 :   double used_acceleration;
+     923           0 :   double used_speed;
+     924             : 
+     925             :   if (taking_off_) {
+     926        4480 : 
+     927           0 :     used_speed        = _takeoff_speed_;
+     928           0 :     used_acceleration = _takeoff_acceleration_;
+     929             : 
+     930             :     if (used_speed > constraints.vertical_ascending_speed) {
+     931       11336 :       used_speed = constraints.vertical_ascending_speed;
+     932             :       ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating takeoff speed");
+     933       11336 :     }
+     934             : 
+     935        7105 :     if (used_acceleration > constraints.vertical_ascending_acceleration) {
+     936        7105 :       used_acceleration = constraints.vertical_ascending_acceleration;
+     937             :       ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating takeoff acceleration");
+     938             :     }
+     939             : 
+     940        4231 :   } else if (landing_) {
+     941        4231 : 
+     942             :     if (elanding_) {
+     943        4231 : 
+     944           0 :       used_speed        = _elanding_speed_;
+     945           0 :       used_acceleration = _elanding_acceleration_;
+     946             : 
+     947             :     } else {
+     948        4231 : 
+     949           0 :       used_speed        = _landing_speed_;
+     950           0 :       used_acceleration = _landing_acceleration_;
+     951             : 
+     952             :       if (used_speed > constraints.vertical_descending_speed) {
+     953             :         used_speed = constraints.vertical_descending_speed;
+     954             :         ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating landing speed");
+     955             :       }
+     956             : 
+     957           0 :       if (used_acceleration > constraints.vertical_descending_acceleration) {
+     958           0 :         used_acceleration = constraints.vertical_descending_acceleration;
+     959             :         ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating landing acceleration");
+     960             :       }
+     961             :     }
+     962       15816 : 
+     963             :   } else {
+     964             : 
+     965             :     // TODO take this from constraints
+     966       15816 :     used_speed        = _vertical_speed_;
+     967             :     used_acceleration = _vertical_acceleration_;
+     968       15816 :   }
+     969             : 
+     970             :   // set the right heading
+     971       15816 :   double tar_z = goal_z - state_z;
+     972             : 
+     973             :   // set the right vertical direction
+     974       15816 :   {
+     975       15816 :     std::scoped_lock lock(mutex_state_);
+     976       15816 : 
+     977             :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+     978             :   }
+     979       31632 : 
+     980             :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+     981       15816 : 
+     982             :   // calculate the time to stop and the distance it will take to stop [vertical]
+     983       15816 :   double vertical_t_stop    = current_vertical_speed / used_acceleration;
+     984        3902 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+     985        3902 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+     986             : 
+     987       11914 :   {
+     988             :     std::scoped_lock lock(mutex_state_);
+     989             : 
+     990             :     current_vertical_speed_ += used_acceleration * _tracker_dt_;
+     991             : 
+     992             :     if (current_vertical_speed_ >= used_speed) {
+     993             :       current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     994             :       current_vertical_acceleration_ = 0;
+     995             :     } else {
+     996       15816 :       current_vertical_acceleration_ = used_acceleration;
+     997        4480 :     }
+     998             :   }
+     999             : 
+    1000          20 :   // stopping condition to change to decelerate state
+    1001             :   //
+    1002          20 :   // It does not apply if landing or elanding, cause,
+    1003             :   // it could potentially stop in mid air if odometry jumps (this happened),
+    1004             :   // Instead, landing and elanding is stopped by sensing the throttle.
+    1005          20 :   if (!elanding_ && !landing_) {
+    1006             :     if (fabs(state_z + stop_dist_z - goal_z) < (2 * (used_speed * _tracker_dt_))) {
+    1007             : 
+    1008       15816 :       {
+    1009             :         std::scoped_lock lock(mutex_state_);
+    1010             : 
+    1011             :         current_vertical_acceleration_ = 0;
+    1012             :       }
+    1013             : 
+    1014        4480 :       changeStateVertical(DECELERATING_STATE);
+    1015             :     }
+    1016             :   }
+    1017             : }
+    1018        4480 : 
+    1019             : //}
+    1020        4480 : 
+    1021             : /* //{ decelerateVertical() */
+    1022           0 : 
+    1023             : void LandoffTracker::decelerateVertical(void) {
+    1024           0 : 
+    1025             :   double used_acceleration;
+    1026           0 : 
+    1027             :   if (taking_off_) {
+    1028             : 
+    1029             :     used_acceleration = _takeoff_acceleration_;
+    1030           0 : 
+    1031             :   } else if (landing_) {
+    1032             : 
+    1033             :     if (elanding_) {
+    1034           0 : 
+    1035             :       used_acceleration = _elanding_acceleration_;
+    1036             : 
+    1037             :     } else {
+    1038        8960 : 
+    1039             :       used_acceleration = _landing_acceleration_;
+    1040        4480 :     }
+    1041             : 
+    1042        4480 :   } else {
+    1043          20 :     used_acceleration = _vertical_acceleration_;
+    1044             :   }
+    1045        4460 : 
+    1046             :   {
+    1047             :     std::scoped_lock lock(mutex_state_);
+    1048             : 
+    1049        4480 :     current_vertical_speed_ -= used_acceleration * _tracker_dt_;
+    1050             : 
+    1051        4480 :     if (current_vertical_speed_ < 0) {
+    1052             :       current_vertical_speed_ = 0;
+    1053             :     } else {
+    1054          20 :       current_vertical_acceleration_ = -used_acceleration;
+    1055             :     }
+    1056          20 :   }
+    1057             : 
+    1058             :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1059          20 : 
+    1060             :   if (current_vertical_speed == 0) {
+    1061        4480 : 
+    1062             :     {
+    1063             :       std::scoped_lock lock(mutex_state_);
+    1064             : 
+    1065             :       current_vertical_acceleration_ = 0;
+    1066             :     }
+    1067       20296 : 
+    1068             :     changeStateVertical(STOPPING_STATE);
+    1069             :   }
+    1070       20296 : }
+    1071             : 
+    1072       20296 : //}
+    1073       20296 : 
+    1074             : /* //{ stopHorizontal() */
+    1075       20296 : 
+    1076       20296 : void LandoffTracker::stopHorizontal(void) {
+    1077             : 
+    1078       20296 :   {
+    1079             :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1080       20296 : 
+    1081           0 :     double new_state_x = 0.95 * state_x_ + 0.05 * goal_x_;
+    1082             :     double new_state_y = 0.95 * state_y_ + 0.05 * goal_y_;
+    1083             : 
+    1084       20296 :     double dist_x = new_state_x - state_x_;
+    1085           0 :     double dist_y = new_state_y - state_y_;
+    1086             : 
+    1087             :     double dt = 1.0 / _main_timer_rate_;
+    1088       20296 : 
+    1089       20296 :     if (std::abs(dist_x / dt) > 1.0) {
+    1090             :       dist_x = mrs_lib::signum(dist_x) * (1.0 * dt);
+    1091       20296 :     }
+    1092             : 
+    1093       20296 :     if (std::abs(dist_y / dt) > 1.0) {
+    1094             :       dist_y = mrs_lib::signum(dist_y) * (1.0 * dt);
+    1095             :     }
+    1096             : 
+    1097             :     state_x_ += dist_x;
+    1098             :     state_y_ += dist_y;
+    1099           0 : 
+    1100             :     current_horizontal_acceleration_ = 0;
+    1101             :   }
+    1102           0 : }
+    1103             : 
+    1104           0 : //}
+    1105             : 
+    1106           0 : /* //{ stopVertical() */
+    1107             : 
+    1108           0 : void LandoffTracker::stopVertical(void) {
+    1109             : 
+    1110           0 :   {
+    1111           0 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1112             : 
+    1113             :     double new_state_z = 0.95 * state_z_ + 0.05 * goal_z_;
+    1114           0 : 
+    1115             :     double dist_z = new_state_z - state_z_;
+    1116           0 : 
+    1117             :     double dt = 1.0 / _main_timer_rate_;
+    1118           0 : 
+    1119             :     if (std::abs(dist_z / dt) > 1.0) {
+    1120             :       dist_z = mrs_lib::signum(dist_z) * (1.0 * dt);
+    1121             :     }
+    1122             : 
+    1123             :     state_z_ += dist_z;
+    1124             : 
+    1125             :     current_vertical_acceleration_ = 0;
+    1126       23266 :   }
+    1127             : }
+    1128       23266 : 
+    1129             : //}
+    1130       23266 : 
+    1131           0 : // | --------------------- timer routines --------------------- |
+    1132             : 
+    1133             : /* //{ timerMain() */
+    1134             : 
+    1135       46532 : void LandoffTracker::timerMain(const ros::TimerEvent& event) {
+    1136       23266 : 
+    1137       23266 :   std::scoped_lock lock(mutex_main_timer_);
+    1138       23266 : 
+    1139       46532 :   if (!is_active_) {
+    1140             :     return;
+    1141             :   }
+    1142       23266 : 
+    1143       23266 :   // copy member variables
+    1144       23266 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1145             :   auto [state_x, state_y, state_z, current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] = mrs_lib::get_mutexed(
+    1146       69798 :       mutex_state_, state_x_, state_y_, state_z_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+    1147       69798 :   auto [goal_x, goal_y, goal_z] = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1148             :   auto last_control_output      = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1149       23266 : 
+    1150             :   double uav_x, uav_y, uav_z;
+    1151       23266 :   uav_x = uav_state.pose.position.x;
+    1152             :   uav_y = uav_state.pose.position.y;
+    1153             :   uav_z = uav_state.pose.position.z;
+    1154        9473 : 
+    1155        9473 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _main_timer_rate_, 0.002, event);
+    1156        9473 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1157        9473 : 
+    1158             :   bool takeoff_saturated = false;
+    1159        9473 : 
+    1160             :   if (taking_off_) {
+    1161             : 
+    1162          62 :     // calculate the vector
+    1163          62 :     double err_x      = uav_x - state_x;
+    1164          62 :     double err_y      = uav_y - state_y;
+    1165             :     double err_z      = uav_z - state_z;
+    1166             :     double error_size = sqrt(pow(err_x, 2) + pow(err_y, 2) + pow(err_z, 2));
+    1167          62 : 
+    1168             :     if (error_size > _max_position_difference_) {
+    1169             : 
+    1170             :       // calculate the potential next step
+    1171          62 :       double future_state_x = state_x + cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1172             :       double future_state_y = state_y + sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1173          62 :       double future_state_z = state_z + current_vertical_direction * current_vertical_speed * _tracker_dt_;
+    1174             : 
+    1175             :       // if the step would lead to a greater control error than the threshold
+    1176             :       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) {
+    1177             : 
+    1178             :         // set this to true... later, we will not update the model if this is true, thus the tracker's motion will stop
+    1179             :         // => the tracker will wait for the controller
+    1180        9473 :         takeoff_saturated = true;
+    1181             : 
+    1182         451 :         ROS_WARN_THROTTLE(
+    1183         451 :             0.1, "[LandoffTracker]: position difference %.3f > %.3f, saturating the motion. Reference: x=%.2f, y=%.2f, z=%.2f, Odometry: %.2f, %.2f, %.2f",
+    1184             :             error_size, _max_position_difference_, future_state_x, future_state_y, future_state_z, uav_x, uav_y, uav_z);
+    1185             :       }
+    1186             :     }
+    1187       23266 : 
+    1188             :     // saturate while ramping up during takeoff
+    1189       22753 :     if (last_control_output.diagnostics.ramping_up) {
+    1190             : 
+    1191         673 :       ROS_INFO_THROTTLE(1.0, "[LandoffTracker]: waiting for the controller to rampup");
+    1192             :       takeoff_saturated = true;
+    1193         673 :     }
+    1194         673 :   }
+    1195             : 
+    1196             :   if (!takeoff_saturated) {
+    1197       20296 : 
+    1198             :     switch (current_state_horizontal_) {
+    1199       20296 : 
+    1200       20296 :       case STOP_MOTION_STATE: {
+    1201             : 
+    1202             :         stopHorizontalMotion();
+    1203        1784 :         break;
+    1204             :       }
+    1205        1784 : 
+    1206             :       case STOPPING_STATE: {
+    1207             : 
+    1208             :         stopHorizontal();
+    1209       22753 :         break;
+    1210             :       }
+    1211         673 : 
+    1212             :       default: {
+    1213         673 : 
+    1214         673 :         break;
+    1215             :       }
+    1216             :     }
+    1217       15816 : 
+    1218             :     switch (current_state_vertical_) {
+    1219       15816 : 
+    1220       15816 :       case STOP_MOTION_STATE: {
+    1221             : 
+    1222             :         stopVerticalMotion();
+    1223        4480 :         break;
+    1224             :       }
+    1225        4480 : 
+    1226        4480 :       case ACCELERATING_STATE: {
+    1227             : 
+    1228             :         accelerateVertical();
+    1229           0 :         break;
+    1230             :       }
+    1231           0 : 
+    1232           0 :       case DECELERATING_STATE: {
+    1233             : 
+    1234             :         decelerateVertical();
+    1235        1784 :         break;
+    1236             :       }
+    1237        1784 : 
+    1238             :       case STOPPING_STATE: {
+    1239             : 
+    1240             :         stopVertical();
+    1241             :         break;
+    1242       23266 :       }
+    1243         693 : 
+    1244             :       default: {
+    1245             : 
+    1246             :         break;
+    1247             :       }
+    1248             :     }
+    1249          57 :   }
+    1250             : 
+    1251          33 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1252          33 :     if (fabs(current_vertical_speed) <= 0.1 && fabs(current_horizontal_speed) <= 0.1) {
+    1253             : 
+    1254             :       // if the current motion was stopped (the conditions above) but we still have a goal (landing or taking off)
+    1255             :       // -> we should start accelerating towards the goal in the vertical direction
+    1256          24 :       // This is important, do not modify without testing, otherwise your landing routine may crash into the ground
+    1257             :       // while having large lateral speed
+    1258             :       if (have_goal_) {
+    1259          24 : 
+    1260             :         changeStateVertical(ACCELERATING_STATE);
+    1261          24 :         changeStateHorizontal(STOPPING_STATE);
+    1262          24 : 
+    1263             :       } else {
+    1264             : 
+    1265             :         changeState(STOPPING_STATE);
+    1266             : 
+    1267             :         {
+    1268       23266 :           std::scoped_lock lock(mutex_state_);
+    1269             : 
+    1270          44 :           current_horizontal_speed_ = 0;
+    1271             :           current_vertical_speed_   = 0;
+    1272           0 :         }
+    1273           0 :       }
+    1274             :     }
+    1275           0 :   }
+    1276             : 
+    1277           0 :   if (current_state_vertical_ == STOPPING_STATE && current_state_horizontal_ == STOPPING_STATE) {
+    1278             : 
+    1279          44 :     if (fabs(state_x - goal_x) > 1.0 || fabs(state_y - goal_y) > 1.0 || fabs(state_z - goal_z) > 1.0) {
+    1280             : 
+    1281             :       ROS_ERROR("[LandoffTracker]: distance to the goal is too large when STOPPING, this could have been caused by a race condition!");
+    1282          44 :       ROS_ERROR("[LandoffTracker]: call for Tomas!!");
+    1283             : 
+    1284          44 :       cause_failsafe_ = true;
+    1285          24 : 
+    1286          24 :       changeState(HOVER_STATE);
+    1287          24 : 
+    1288             :     } else if (fabs(state_x - goal_x) < 0.1 && fabs(state_y - goal_y) < 0.1 && fabs(state_z - goal_z) < 0.1) {
+    1289             : 
+    1290          44 :       {
+    1291          44 :         std::scoped_lock lock(mutex_state_);
+    1292             : 
+    1293             :         if (!taking_off_) {
+    1294          44 :           state_x_ = goal_x;
+    1295             :           state_y_ = goal_y;
+    1296          44 :           state_z_ = goal_z;
+    1297             :         }
+    1298             : 
+    1299             :         current_horizontal_speed_ = 0;
+    1300       23266 :         current_vertical_speed_   = 0;
+    1301             :       }
+    1302           0 : 
+    1303             :       changeState(HOVER_STATE);
+    1304           0 : 
+    1305           0 :       have_goal_ = false;
+    1306           0 :     }
+    1307             :   }
+    1308           0 : 
+    1309             :   if (current_state_horizontal_ == LANDED_STATE && current_state_vertical_ == LANDED_STATE) {
+    1310             :     {
+    1311             :       std::scoped_lock lock(mutex_state_);
+    1312             : 
+    1313             :       state_x_ = goal_x = uav_x;
+    1314             :       state_y_ = goal_y = uav_y;
+    1315             :       state_z_ = goal_z = uav_z;
+    1316             : 
+    1317       23266 :       have_goal_ = false;
+    1318             :     }
+    1319       22753 :   }
+    1320             : 
+    1321       22753 :   // --------------------------------------------------------------
+    1322       22753 :   // |              motion saturation during takeoff              |
+    1323       22753 :   // --------------------------------------------------------------
+    1324             : 
+    1325             :   // update the inner states
+    1326             :   if (!takeoff_saturated) {
+    1327             :     {
+    1328             :       std::scoped_lock lock(mutex_state_);
+    1329             : 
+    1330             :       state_x_ += cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1331             :       state_y_ += sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1332             :       state_z_ += current_vertical_direction * current_vertical_speed * _tracker_dt_;
+    1333       46532 :     }
+    1334             :   }
+    1335             : 
+    1336             :   // --------------------------------------------------------------
+    1337       23266 :   // |                        heading tracking                        |
+    1338           0 :   // --------------------------------------------------------------
+    1339             : 
+    1340       23266 :   // compute the desired heading rate
+    1341             :   {
+    1342       23266 :     std::scoped_lock lock(mutex_state_);
+    1343           0 : 
+    1344       23266 :     double current_heading_rate;
+    1345           0 : 
+    1346             :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1347             :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1348             :     else
+    1349       23266 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1350             : 
+    1351       23266 :     if (current_heading_rate > _heading_rate_) {
+    1352           0 :       current_heading_rate = _heading_rate_;
+    1353       23266 :     } else if (current_heading_rate < -_heading_rate_) {
+    1354           0 :       current_heading_rate = -_heading_rate_;
+    1355             :     }
+    1356             : 
+    1357       23266 :     // flap the resulted state_heading_ aroud PI
+    1358       23266 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1359             : 
+    1360             :     if (state_heading_ > M_PI) {
+    1361             :       state_heading_ -= 2 * M_PI;
+    1362             :     } else if (state_heading_ < -M_PI) {
+    1363             :       state_heading_ += 2 * M_PI;
+    1364             :     }
+    1365             : 
+    1366       23266 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1367             :       state_heading_ = goal_heading_;
+    1368       11949 :     }
+    1369             :   }
+    1370       11949 : 
+    1371             :   // --------------------------------------------------------------
+    1372             :   // |                      landing setpoint                      |
+    1373             :   // --------------------------------------------------------------
+    1374             : 
+    1375             :   if (landing_) {
+    1376             :     {
+    1377             :       std::scoped_lock lock(mutex_goal_);
+    1378             : 
+    1379             :       goal_z_ = uav_z + _landing_reference_;
+    1380             :     }
+    1381          20 :   }
+    1382             : }
+    1383          40 : 
+    1384             : //}
+    1385             : 
+    1386          40 : // | ------------------------ callbacks ----------------------- |
+    1387             : 
+    1388          20 : /* //{ callbackTakeoff() */
+    1389             : 
+    1390             : bool LandoffTracker::callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    1391          20 : 
+    1392          20 :   std::stringstream ss;
+    1393          20 : 
+    1394             :   // copy member variables
+    1395          20 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1396           0 : 
+    1397           0 :   double uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1398           0 : 
+    1399           0 :   double uav_x, uav_y, uav_z;
+    1400           0 :   uav_x = uav_state.pose.position.x;
+    1401             :   uav_y = uav_state.pose.position.y;
+    1402             :   uav_z = uav_state.pose.position.z;
+    1403          20 : 
+    1404           0 :   if (!is_active_) {
+    1405           0 :     ss << "can not takeoff, the tracker is not active";
+    1406           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1407           0 :     res.success = false;
+    1408           0 :     res.message = ss.str();
+    1409             :     return true;
+    1410             :   }
+    1411          20 : 
+    1412             :   if (!callbacks_enabled_) {
+    1413           0 :     ss << "can not takeoff, the callbacks are disabled";
+    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             :   if (req.goal < 0.5 || req.goal > 10.0) {
+    1421          40 : 
+    1422             :     ss << "can not takeoff, the goal should be within [0.5, 10.0] m!";
+    1423          20 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1424          20 :     res.success = false;
+    1425             :     res.message = ss.str();
+    1426          20 :     return true;
+    1427          20 :   }
+    1428             : 
+    1429          20 :   {
+    1430          20 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+    1431             : 
+    1432          20 :     state_x_ = uav_x;
+    1433          20 :     goal_x_  = uav_x;
+    1434             : 
+    1435          20 :     state_y_ = uav_y;
+    1436          20 :     goal_y_  = uav_y;
+    1437          20 : 
+    1438             :     state_z_ = uav_z;
+    1439          20 :     goal_z_  = uav_z + req.goal;
+    1440             : 
+    1441             :     state_heading_ = uav_heading;
+    1442          20 :     goal_heading_  = uav_heading;
+    1443             : 
+    1444          20 :     speed_x_                = 0;
+    1445          20 :     speed_y_                = 0;
+    1446          20 :     current_vertical_speed_ = 0;
+    1447             : 
+    1448          20 :     have_goal_ = true;
+    1449          20 :   }
+    1450             : 
+    1451          20 :   ROS_INFO("[LandoffTracker]: taking off");
+    1452             : 
+    1453          20 :   taking_off_ = true;
+    1454             :   landing_    = false;
+    1455             :   elanding_   = false;
+    1456             : 
+    1457             :   res.success = true;
+    1458             :   res.message = "taking off";
+    1459             : 
+    1460           5 :   changeState(STOP_MOTION_STATE);
+    1461             : 
+    1462          10 :   return true;
+    1463             : }
+    1464          10 : 
+    1465             : //}
+    1466             : 
+    1467          10 : /* //{ callbackLand() */
+    1468             : 
+    1469           5 : bool LandoffTracker::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1470           0 : 
+    1471           0 :   std::scoped_lock lock(mutex_main_timer_);
+    1472           0 : 
+    1473           0 :   std::stringstream ss;
+    1474           0 : 
+    1475             :   // copy member variables
+    1476             :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1477             : 
+    1478           5 :   if (!is_active_) {
+    1479             :     ss << "can not land, the tracker is not active";
+    1480           5 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1481             :     res.success = false;
+    1482             :     res.message = ss.str();
+    1483           5 :     return true;
+    1484             :   }
+    1485           5 : 
+    1486           5 :   {
+    1487           5 :     std::scoped_lock lock(mutex_goal_);
+    1488           5 : 
+    1489             :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1490           5 :   }
+    1491           5 : 
+    1492             :   ROS_INFO("[LandoffTracker]: landing");
+    1493           5 : 
+    1494             :   landing_    = true;
+    1495           5 :   elanding_   = false;
+    1496             :   taking_off_ = false;
+    1497             :   have_goal_  = true;
+    1498             : 
+    1499             :   res.success = true;
+    1500             :   res.message = "landing";
+    1501             : 
+    1502           8 :   changeState(STOP_MOTION_STATE);
+    1503             : 
+    1504          16 :   return true;
+    1505             : }
+    1506          16 : 
+    1507             : //}
+    1508             : 
+    1509          16 : /* //{ callbackELand() */
+    1510             : 
+    1511           8 : bool LandoffTracker::callbackELand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1512             : 
+    1513           0 :   std::scoped_lock lock(mutex_main_timer_);
+    1514           0 : 
+    1515           0 :   std::stringstream ss;
+    1516           0 : 
+    1517           0 :   // copy member variables
+    1518           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1519           0 : 
+    1520           0 :   if (!is_active_) {
+    1521           0 : 
+    1522             :     ss << "can not eland, the tracker is not active";
+    1523             :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1524             :     res.success = false;
+    1525           8 :     res.message = ss.str();
+    1526             :     taking_off_ = false;
+    1527           8 :     landing_    = false;
+    1528             :     elanding_   = false;
+    1529             :     changeState(LANDED_STATE);
+    1530           8 :     return true;
+    1531             :   }
+    1532           8 : 
+    1533           8 :   {
+    1534           8 :     std::scoped_lock lock(mutex_goal_);
+    1535           8 : 
+    1536             :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1537           8 :   }
+    1538           8 : 
+    1539             :   ROS_WARN("[LandoffTracker]: emergency landing");
+    1540           8 : 
+    1541             :   landing_    = true;
+    1542           8 :   elanding_   = true;
+    1543             :   taking_off_ = false;
+    1544             :   have_goal_  = true;
+    1545             : 
+    1546             :   res.success = true;
+    1547             :   res.message = "elanding";
+    1548             : 
+    1549             :   changeState(STOP_MOTION_STATE);
+    1550             : 
+    1551             :   return true;
+    1552         107 : }
+    1553             : 
+    1554             : //}
+    1555             : 
+    1556             : }  // namespace landoff_tracker
+    1557             : 
+    1558             : }  // namespace mrs_uav_trackers
+    1559             : 
+    1560             : #include <pluginlib/class_list_macros.h>
+    1561             : 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..ccc761657e --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html @@ -0,0 +1,411 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ 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..8481bd4d72c006ac4b214dd4f28e61d3fc42c8b3 GIT binary patch literal 5276 zcmV;N6l3d&P)b7Rm(J1T|M3%GeX5p=wJT&yvqEDij|jrjGUmMifxP3YF6`P+}bRMh^WD z?_+RB+6(73RCgu%=7=8JBNhfURy__)nG7A%ukH(aCWudKymYUbRjp#bpz6U%q^rBf z3=;6Q0e7MSjQy#;!SV-7Dy(W@g$$1qD_}otX!g_AU%H|$l zTmCKq(9tqXT^Px2^>L?+eqpa8zT0wZ#J>frbgY0!Jka79YU7Z)BUlWC@*`A_H>_VK zRK+B~R@G^i*zJ@i;2S~*gbSablqO7qrE#wQ)P>%F^2?DZOb3YJjr3-3+0GnrL;)ul zaI63th{;`26@-NkuvRgX+YTK_EXzbP0O+~}02~#-gKbn>31fs@dj%kju{R(}j0T4n zI+f-sK7lAv`?CSvqoN9p1c42^c8R`RmA9;f%K#`~fNZ}~U{EHk=eqpbGet(d0c^irJL{5^g zPE2uAFKv~t3p3uFXk*h`6aSbW_b5`z^f>%vZ%(2$Io_S_~YXn>cK(Kg>3nRV6XT$hz znS|nG8|!SbW4y^GE*Y0e&Qdo;Cjk*x0tjM+iyt)O6xICT=l9{*pdozBMV)!Fw1qXiKF&0RPp2h&KWkW*c#76YK6 zubsy6Ki3l3Z0+l6Vb~3T|Em61_19Is^e1Myf<65)UQl&yX4|=v0KZ}S@a%aTXS#iM zv}d9^Uvj)gjPHkK(M`Uq0C)bh0KxJ-Z6u=v0$gVe@KDtXRRv&;PlpA!O3g8hYZzb6 zs#U-vRcm!;RBM1})&0FQv+oeGq$R3BhhNl2%EH7`_9+0aa|W1`7cu65`Sf#`>Pe3q z#Hns}&G9x4x&Qij&up#KG~~G{?%bG}UAMLY0jGxpgv%wT3%pmM7e+4mqO`3x1u&=g zRF)nx3v)-pL5y#+@VT3cyTw>H{R%^Kvn5@5z1;JQ8=3@4M0<^&BJCt-aI^2`LXW1N zH}WxUv2_!5R1M9#f+YG89N4$V&n^tj{<&ct=!9Yyv!@q`ut3SHD8($7F9!HIF;k1^ zb&ZF%{*2cZeIxn(<5)Op<9g-n{Jq+~)tr_eYyb*XUjvvlKy73gRCB1hu({3wl1BP2 zIY)#EBBCSm=!@Z#l~2%p3skCe;?`Ve0|10F`-C*%7)Ie zM8I7%C=U8es|El+FJ@tviR?6uyyq?zv+)EPQhtG$Wm3jItX%hGGgADx%iM1einMv1 zqM$~`>0f0@6%OeGUFgQUYNwn5lm+dAzK&+kS`qD7bszu(bQpD~3jqW$dJd_8V4-fD z1|W*@gOk0*q4F1PnsER)x3N5UT@%gSJ_{${?7VD)5WH!5@r%L&R;DHO<1-X*cBpZB`y)ao;RVgr)t$eaA32nQo6)#Td&CB(bjrhrpFo ztDE~3d6eeQ+cw}E$aIWyXN|^bOJz-wa+Y~wLW5foLn_9|OoH)T88myVDB;TII`(wU66|ldH1Uao}#7ItzYLVXtj6!6cuqIxQ zMjN z2t&fqe2IV!j39d+;QV0_3b<;o@SZ<`;8&o2{~6@UsP?wEG7-;2{^5eYwvm`8b1u@v zp@5IrXjbDX_WbM&`4FIoRRzu7*HHmH-QZ6>WXHI7HgLANJ7CZhL%$0GxIG)~+W2Iy zgzEZN7-RxvpR`C}qTs48u}GgmtLT8Jp$S)A8k&_ZG>92^Rx0Gt6kIrAS!Jko@4~GA zU()l_u}7Dj*I_>Tx9fm7MoGTQUb-TD9=9J$?wzEXGg1)l*RJI=YcHRSMU4CB5~FYW zMJx8crwbx2uLoPm6~+hQP6Fgm+o!0SH`dVElR&Sm10tN8kroTG^NxvbQqBnc+xiB&#HlIjmJY zzCG7PZNOe_$Y;V@kABiNgUgk{oUT;2Ut#v(Mx4Y)3F8;?u^y}WkB>UWOdhL@{KrQf zBjckIQ~~cHfjf}*GHZHG;lo7ZZMwEbMmepc(1lW>$a4XwrEONZHyI~pP7Kn5mP}R(5ei>KVzT|_^W)W3 zjZqB~g8};cd(Kk8%LMK*fSMGJVTY*}4#uug9c=M|UKf0cHlwPd9`|`(_hhQar~uN~ zS8nPO^5b?mOwQBx)Y@nf@DNa?3ogy|r?wGS0GRmLh(Q&=*N*FKSbua}!=?X$cj=17 zQJantlpI?PPCVl$v8TVmeyRQFj;P$3jyY{V_Q;xE(c#5OXiI!J3W%N?hD(+SG1KQM zLAfn7J+tDfV;~psp*9|ibyEd!#0K0f2`G@uNR2jB5&*k2A@aP)jkiWNoa$8Hl;>mxK0pU+>2TbI(8@_O$aV zfEXW=6E5(Pe3!-KrBgu`Vo+ZSDn0NiUqcC5xkf$CZccd}(_V|%#){Sxh(W0a zi}$(=ju4gMPO0ws2Ue+Z=c?Aur}SS`z-`p-1z>wfhCE1U)!FJym=O2BTN8rp%Sh)O zT)hpsCmu=u!YoMyt(W9^pL1VxYG9two99B^@4gzXemoI0!h! zX;iLTs6Ly8u3TyI?Nr!)yeLDJ_k%Aadrpz#9NSl%eH!K^q>m05i{+IEOm}^{(dC;H z|1yV5(cJ+A7oZidq6w>2N=)0>B<8j6k^R>aZcRyP^l(0ceYg)-WO(6KZv~cI8`@Tx zOaG%t%>}-cFN@LC{g1GDEl2H@hx88yrm5}+9HYnA$j)aGw5Y#n2vR8mcpG0o{+c|! zm8Lc-Q_!I{qAN{9>AljND~7nRht%CgV$X#5$JYvm`8ulZDBhNSmAa622Z@nhIQ7ST zQjVE2`?Fa=xf8jM&z@J^+mgb-TmTEZadyf}Fg9U~oZm~0-=;3agF|`?WOpOQKs8{Z z%zxWP&J_nlgjAPuCT$SonI);1R@1ms;q%cv)9F+9z^7pl5H3Ec{GF{fBtC|!S>Bn7 zQyQ}=e9oJZ>Zj9#E9KDNX;Ei|!qHJAc)WHurBfTPI*RO8wtWB%hGxE?c#0`eaH78? z9bw4r*D2kWus9Rbr+vjSe-e>D3|?KthvQ<_Rhv1CZ$xB$Q~|^>lAIB*YKnP342R5{ zI7+GIKBy^77y;7^p0B^bKw_Che8MuCe&v;f0+gOBa>eH+SF-7j#o3FON}My|OqU!0 z-FV5?kTGIN3J#&S3nswu2Oe=6pVHcO6@HKx0>(A`D|W9|4j5ZgI|pe#0pU4R@1i~D zKCkX9;7v-*c(Si!jgh<3D1t8XTCIF!j+QQ$T6%O1K)B3gQ?qg7mt)lI?qjWPVP624 zjE^WAV@Q@5e@`Z%h^!HvTt_sOwbY(ofC<(TxVtA}ci|aB zV6y)gZcg*&Mm3q$Kz^I}=qApvp*M}OMi=Zb?*DG2k?B4+>Eeoc<}Tk3qmW<(d?~q? zB5bKHOL&fe_i`d&zc+e576=qKhvPo({!jxf%X-v1Gc%tZ<7N!P*gT^7284FB@q?aW znzyf0&*YZj>3b@e)OI)`x)0Y>^h7`Uuu4_qd+aD6bVwW=fWEXz{smM6n`dVI2mxZm zT(k=S9P|A>V@Bb?JpYa{qX*f57-6cnMRvu-le=*+&zW&EKKTo(K?9~p>F?_6-ukgU zz#%*GBB~MnXr&s0>uv2_aD<*Op~#Y#g@6cV;j-S>{JZtgG9@h!eA;ura@ZFULjW9T z+M*CHIFj3;_r6}}oq@r~y@%Oii9Ue)61*fHLJUp6MSHKE`vGM5jD^|N0A-A5)lirX zD6_usWuxs~xj=(qru96{Uhic{@JEd3NOY;6~&M=1y^QqAu9AWQgyVFi>{`i z!^vlSsLtIZrD0$|FeKt3397#xp=|-m=mz(BF&Zc2I$`j zdM|8T`ma|rMzoE$U_(#M4g@`#zd&%w<|0okNA5a044A?C=$H?l=Yp>qnYmT(#f&NZ zy>W)?P*Alu`+Nf->%dYOv=^LM#M_v83#(P*%APVZhMsH>8Q_gh*HOBLXt$8j0S=K_ zx(hyT8ODu8%N^ZJ`7Biz5uey2)gr@J?m!@iV*!li+>B4`aPhtZ8t$=5(bY*FDM9(k zi`iznPE-=qLlXgRY^0VSi7sn-wGGq`p6@JKx6Q!6j`paVL^WuK%cXp@YNx=RAe((X~&E$H8>$dmX`Qk0quN;sU zn)Qd=u4}oYI{`M1RlDkRQY?orOcI`~6OG{|AHy%!?yunE6_?A0`*eqJOhPuL@SY(_ zG@QX1Qgg)A7!i;cnpC3!mha>D0#BiaiUxJ#bw;+ZY$k87sz@ zeni%`EA~?5Mur`q zziQ?RvD}h-%{#RYRPET)0)#OVr;FmddfaYV02}eM`*f#izaS25eYl!@0JvA7ZLxd6 zwkFM~ZV>Zt8vf`sEY8K3PQwV`H%-HRYR`@IE*J;INa#Ne_rGO%bne@+A9pg09gHca i-!-hoicBb5yZ!@VT1kj-+z + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-06-08 22:13:12Functions:193063.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::line_tracker::LineTracker::deactivate()0
mrs_uav_trackers::line_tracker::LineTracker::resetStatic()0
mrs_uav_trackers::line_tracker::LineTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::line_tracker::LineTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::line_tracker::LineTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()1
mrs_uav_trackers::line_tracker::LineTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)1
mrs_uav_trackers::line_tracker::LineTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::line_tracker::LineTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)1
mrs_uav_trackers::line_tracker::LineTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::line_tracker::LineTracker::changeState(mrs_uav_trackers::line_tracker::States_t)4
mrs_uav_trackers::line_tracker::LineTracker::changeStateVertical(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::changeStateHorizontal(mrs_uav_trackers::line_tracker::States_t)6
mrs_uav_trackers::line_tracker::LineTracker::stopVerticalMotion()19
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontalMotion()19
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontal()46
mrs_uav_trackers::line_tracker::LineTracker::decelerateVertical()100
mrs_uav_trackers::line_tracker::LineTracker::decelerateHorizontal()100
mrs_uav_trackers::line_tracker::LineTracker::getStatus()181
mrs_uav_trackers::line_tracker::LineTracker::accelerateVertical()200
mrs_uav_trackers::line_tracker::LineTracker::stopVertical()850
mrs_uav_trackers::line_tracker::LineTracker::accelerateHorizontal()1004
mrs_uav_trackers::line_tracker::LineTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)1703
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2126
+
+
+ + + +
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..8922c6d320 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-06-08 22:13:12Functions:193063.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8ab26f243a7db7dc64cc13c3281b58741f953d67 GIT binary patch literal 4090 zcmVs!$S`IZ5?&jpY<)Q^<9Kx!q!6pfc-aZnhg=)#LKMpsu|_wp!otoN3l|;00OFR>w{C z3?c97wSk6dfZ}+rCs_Yrdkkx}7C4UkWh}YLwQJv~=XnrK(hehW_NrmRQ(Kl>(#_5( z5TzUGEb%q#!Ef{j)uiHv$sv&rp_owhU*sGXfG4^qfx~4=3cA=bE(}9{ol>O0Kp}l> z0(}Wo%;Xe31;R}cXuza?s=ylqo-JcSv8U0{w2;45W|sUd1e{UefdqhyDOeRVeY8<0Ka4)5k5tE< zr&k0_R599YBQiL@EgSV*_0r+5?% zDJBQv6w7Ls0ZXN4K$w{ZaB$_Y-gUX9BdIer1B|b)0=BTxohf$df#QYkOpN^qO~HTK zrg#}}jEZ$-4FkuQu}SilbzLK~uF^}hKChG+GbQc?;Q6dke-EXYj1^M3_Ky{sW7+U! z$W8V)M>^n+;sw(#GvPY^nft{!hltsrGT!goouw_j?)2knWhwa)1;S}1zgk0PYnB*r z55Qdjg2_?b7#BG`9mRLaBoZgfSQd+(;v@ujS&E$|i7C%sV~M&FAV?8TuF*Ij%FSGw zH*;U3A#yBf?IvhATr(-1s7xA)a?!RF`1A83>3RJB{_}YMXFvFH$pRnblR}z)S_ltR zSK%#q!Pe^c5ABNrPm%(tsB3F++}}MVwR@m~Vkt4|3gBPX1G3)K6yN;^??TcJptFpd z|LaTFP4WG(ZS`L1Dxi^13kas~DI-xV2)M5`U{}@^t}+>o^J|= zfNG}nR9|>{aApX#fxrOH0^xMYVooOu9b3(5zS$$jt&vmhHZ|D3pVWO z$C}w@{RHiC#`xhMWbOHoQlJ&GhW5H84_t1F=1SV@UJ<@R&79@4fuYJ8gmuNDDJE;F zDTrl6fqTO~S>RER){w9cUQ#n025JViMs{C!wF|A1?$#DVLK@iYc-l=0=d8kQ0( zW4Ugd9t-q+5*HCq28hVV3A}Cjq^t|p49$8n%XEM>*I>^7v?=_0lak9w1-w9xlu_Sr z?+Nb|Xugjf{N|tc;|H1!iHBX*+-KMYVD4OO?{rM{^k(gd2z{ael;blUStnc!py(KJ z|4bFqf1YAtb}<&CO7TtxB^p-&vnY6SP3HYl?~<+qwyd5r%}8^NS=*HKO#Q&{p$Dd; zBnmX&M~V;s{5*c3W+qB6?`OrVY2Nnj`-8WHyR&vxB-D-o{3!yJS$i%jKqW=Th#9Fo z%|G+}bd@=Qox;yWyZltqLi1sqzrKaR@s;z}e0Nat*fUT*F^W4I507!2m3u1u-9%0J zMh1S|_HO}bIyFjzKGUoLAUd>HRH0JM{1i8t=3aQsU8-iw2{I)80yRstjJ?^JKBkUy zc)MHYHdv(?{byzP-DukZa%!P*$`aYvvi=jzw;Mo_&wN;o1|| zn>EXbGV>k@mD?Tz^0e>Hy*`n3La_+cPz(*Y!qivI&y1U(A@O79ot^gfoVVJVG1O8F z-lO-%b08SyR%Cr)6%IQyC9A?hpiLw2M}t(a%ie2?EW7D$|`A`7^7y%kqYCtSadF%?xx}0}s)~<@g{u+#3b!6R$ zU0rUP>t#fld6|aU2Eu+1oG$Zw2A=;wIo}+ zjCAyICYCZ}kh_UvybQP|VLdtP@jx9j&5`pp@t(!5Ht}uZ7!0^@+icHIIo6@Xg=vjB zTFcA^EJD00{J=DA|NV#_IV8fN3nANxhSqVvIcr?soz? zHjTvjZ=^EzFjLvpaROL)+g-+cY4?W!ls8szpkoB02m=R~v6}TIX0Nh-JW~9su;Q>| ze7P&eEHmrNI=Aj{wM>8=6fL2`UK?o(6jQmbtarN$kBey|FGtd@VQ(v2VLCODgexPM zb)8}mQtvK}GGiJQPt)n6#>gyb6$fVmj|-bICJ)3ZK1f8ULR~@eB`iwKK6$)4X91qI zp*@Lu9IIZ>Gvh+YPZDSsgUN~*DL;c`!2X7?1SNs};-)Z!_))PvFGp035f8MLJ7aIQ-{JY!9ivV&rqU?RoXso-}U$Oo0lD z*t83r;$EO~oH;Oq0)HO9In{Tu!CV7-5PqVOYR~xKDsImKZNs4%pC_?Bt*Pz{Y|jVf zICkdj<8tI>96`}H==yZ?MEj4~pD-BKld`TN0#RKDG+f%-lYDlGz+La{@_YC-!0t1z zSIS9}<5!7_Cn|p#Fn&>-bJF<~M`*ZRhbZvpW&Ku2j<4Y3)_^E8KeTfTODV`lU3Gmp zhV41_Yk)7m*kVjy=t(~4>mxj=#lt+(TYj<{)1E(%v`o=jq}o8(0u|C}U)Z3W8%;=^ zm*bDvB~bepf#%^A)YUg|zsx~R5wF6~XsBU%#k{pOlL%6}BxGK!d?{{9FYu5knm5IY zzsQ{t1mhpWIvEAU6E!1tB0V9JW#YG@@pr6d$RXy*oi=j-n!2Ji=rgTC0QmWRNX-o* zcq>F}hN{^!CDT>ciq$xwF48LNm$(-!(<^>VF$HFq^u;47t&cujDb%LfB|E^PpXg2V z@G2}XLQKvqz{O*nT%(Dt-TU3ZBc0gF75{M7V5uRg54wSc(@i4`WY_zKZPsuUV6O|ZT zM)ADnEtiFE;l*xQ-^`kh!hx;$8YFq)?w}QH$NPL4vH-l7)+_I-todkIu1=@8&793Q zohy7Lb^~yDQ@{gk0T^@|ZyrU8l_6xjzWcjOO4XuhDBeZY%8q5j<(--o!I~(`xQ6lw zn+k{yD=40@J!e;QuZ~9jl&H&q_kuMcYX}u}ovn7?EuoG4uR2eP?5St~hc~YAd+qzo z-aO`(ypq}Dea=^Sq;xsntF|OYA)TTpRS2do<)7I4EegDJdivK0ye#n{0#7cI1qAM| zf-iRLOvV?)r15Prprs%^+ZNB|i(7rXE$SM57^>&3SxMa4T~RQ_+P06q2;9pX?lg7i zHUZDx8$W)}ReHLJMaQ;Jk4zD+6W_w#55 z6`7yDF*D|Yikhhy_!_W?{IXsl-cdy^z6-qqoVWoPaUYU=(_g+p_umZ8Ms7N#8;__h zI)Jbz^^?pj9t-hHo>Z!~vlx3_@gt6*)2!DtYh%f{RS%h?Dn1%_fqjG%YcrL&%gHt) z)YSL28Qx?_i&0W!-Tq$C;-hk)CL^>Nc{B2vHj}Nw(dzjBw$1$8eg;mfyLY*K5`Zsb z9E~M{;(@ey;m7xZe{4zO{(&z4O#dS~dLOdwp#0 zmaUUY*4gnpkqgH)AMgBlCOw6jBKOD)286})OS+Gm%cY^Xm1t6XF-?-oC0X!Ea*$-- z4D3Eu$%$iy4rK&e*A?;bEh(FM@wbA67mZ0lnE~}oqZGK;j4}hKRCxsbGU*FE0G#4* zuUQN=N&Bo}ingEOtmhrhaS+#92B2^gR8(Cg>5GjSmxzRU4?S60A&VF zpr`N+t^%C?g3=(x#70qDprV-Ws#=!vc{K^Hs6C?wKJjoYuyI{-mR%Bm%!+LOb`7P> z&x;XFnBuRpkDnb%iMao*ef*n%C{w%-atjmtSTm0k$Cb5bjJJ0uz|r;*gJmP6qIvAL zk45vCjSGw3bq*{UivO#nsBbyJ%P1`6I7jj0mV$nC43ho5XED4oS?!^ceGm8C{D*&V zU!+P_Qpo?OnvL6rknK1)N%4bfmT6B>d0=ZcTzlYj@v5e{t||5>YJmrRtS?|Y5o=z^ zYwxl~A0lZ1{dSHf#p`xJRIc$j-Y3_1m>-KL9jkqcaILu*hj*b*KF3~|A?}2FXZK2y_Jy<64st__9_!$VCi5*oz7{sjgSwliX8AvrF3q@#K}GFj8Q$ zRTvLlx!bmohvg&Fv}t?pwXML5F$z3bh + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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..4c896c740b --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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..b4a74470a2 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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..475bacc557 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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..29fbf828f4 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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..c0b813efa6 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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..5f60d3e509 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resetStatic()0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()52
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)158
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()178
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
+
+
+ + + +
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..7b511dc43f --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()178
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>)107
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&)375
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)158
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()52
+
+
+ + + +
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..0f2d91c784 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html @@ -0,0 +1,426 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-06-08 22:13:12Functions: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         107 : 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         107 :   this->common_handlers_  = common_handlers;
+      81         107 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83         107 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85         107 :   nh_ = nh;
+      86             : 
+      87         107 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99         107 :   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         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109         214 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111         107 :   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         107 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122         107 :   is_initialized_ = true;
+     123             : 
+     124         107 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126         107 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133         158 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135         158 :   std::stringstream ss;
+     136             : 
+     137         158 :   is_active_ = true;
+     138             : 
+     139         158 :   ss << "activated";
+     140         158 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142         316 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149         178 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151         178 :   is_active_ = false;
+     152             : 
+     153         178 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154         178 : }
+     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      136027 : 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      136027 :   if (!is_active_) {
+     174      135522 :     return {};
+     175             :   }
+     176             : 
+     177        1515 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179        1515 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181        1010 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183         505 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184         505 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186         505 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187         505 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188         505 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190         505 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191         505 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192         505 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195         505 :     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         505 :   tracker_cmd.use_position_vertical   = true;
+     203         505 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205         505 :   tracker_cmd.use_velocity_vertical   = true;
+     206         505 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208         505 :   tracker_cmd.use_heading = true;
+     209             : 
+     210         505 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212         505 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219          52 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221          52 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223          52 :   tracker_status.active            = is_active_;
+     224          52 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226          52 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233         295 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235         590 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237         295 :   res.message = "callbacks are always disabled";
+     238         295 :   res.success = true;
+     239             : 
+     240         590 :   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         375 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300         750 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302         375 :   res.success = true;
+     303         375 :   res.message = "constraints updated";
+     304             : 
+     305         750 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     306             : }
+     307             : 
+     308             : //}
+     309             : 
+     310             : /* //{ setReference() */
+     311             : 
+     312           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MidairActivationTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     313             : 
+     314           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     315             : }
+     316             : 
+     317             : //}
+     318             : 
+     319             : /* //{ setVelocityReference() */
+     320             : 
+     321           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MidairActivationTracker::setVelocityReference([
+     322             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     323           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     324             : }
+     325             : 
+     326             : //}
+     327             : 
+     328             : /* //{ setTrajectoryReference() */
+     329             : 
+     330           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MidairActivationTracker::setTrajectoryReference([
+     331             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     332           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     333             : }
+     334             : 
+     335             : //}
+     336             : 
+     337             : }  // namespace midair_activation_tracker
+     338             : 
+     339             : }  // namespace mrs_uav_trackers
+     340             : 
+     341             : #include <pluginlib/class_list_macros.h>
+     342         107 : 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..7d2c9155e3 --- /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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
<unnamed>82.9 %1374 / 165790.0 %45 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..e0a63c17a9 --- /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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
<unnamed>82.9 %1374 / 165790.0 %45 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail.html b/mrs_uav_trackers/src/mpc_tracker/index-detail.html new file mode 100644 index 0000000000..8063e84f10 --- /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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
<unnamed>82.9 %1374 / 165790.0 %45 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html b/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html new file mode 100644 index 0000000000..94af55f126 --- /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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html b/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html new file mode 100644 index 0000000000..5c4c984d6d --- /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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index.html b/mrs_uav_trackers/src/mpc_tracker/index.html new file mode 100644 index 0000000000..49c39a0083 --- /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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mpc_tracker.cpp +
82.9%82.9%
+ 		82.9 %1374 / 165790.0 %45 / 50
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..5744dd5959 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackWiggle(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()40
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)98
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)107
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)171
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)260
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)262
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)269
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)338
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)531
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)737
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)739
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)742
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)1226
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1975
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)2151
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)4395
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()9231
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()9232
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)15588
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)22147
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()23466
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)79929
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)85596
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()86608
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)86608
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)86608
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()86608
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)86608
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)186640
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)186640
+
+
+ + + +
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..68d178210b --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()40
mrs_uav_trackers::mpc_tracker::MpcTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)107
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)171
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)1226
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()86608
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)79929
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)260
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)186640
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)742
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1975
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)269
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)86608
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)22147
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)86608
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()86608
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()23466
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintMPCResult(double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)737
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)5
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)739
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)186640
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()9232
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)531
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)2
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)4395
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)338
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)107
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)2151
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)85596
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)15588
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)262
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)98
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)86608
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()9231
+
+
+ + + +
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..fce4f3f5b1 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html @@ -0,0 +1,3955 @@ + + + + + + + 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:1374165782.9 %
Date:2024-06-08 22:13:12Functions:455090.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : #include <mrs_uav_managers/controller.h>
+       8             : 
+       9             : #include <geometry_msgs/Pose.h>
+      10             : #include <geometry_msgs/PoseArray.h>
+      11             : 
+      12             : #include <mrs_msgs/FuturePoint.h>
+      13             : #include <mrs_msgs/FutureTrajectory.h>
+      14             : #include <mrs_msgs/MpcTrackerDiagnostics.h>
+      15             : #include <mrs_msgs/MpcPredictionFullState.h>
+      16             : #include <mrs_msgs/EstimationDiagnostics.h>
+      17             : #include <mrs_msgs/VelocityReference.h>
+      18             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      19             : 
+      20             : #include <std_msgs/String.h>
+      21             : 
+      22             : #include <mrs_lib/profiler.h>
+      23             : #include <mrs_lib/utils.h>
+      24             : #include <mrs_lib/mutex.h>
+      25             : #include <mrs_lib/attitude_converter.h>
+      26             : #include <mrs_lib/subscribe_handler.h>
+      27             : #include <mrs_lib/publisher_handler.h>
+      28             : #include <mrs_lib/geometry/cyclic.h>
+      29             : #include <mrs_lib/geometry/misc.h>
+      30             : #include <mrs_lib/scope_timer.h>
+      31             : 
+      32             : #include <mpc_tracker.h>
+      33             : 
+      34             : #include <dynamic_reconfigure/server.h>
+      35             : #include <mrs_uav_trackers/mpc_trackerConfig.h>
+      36             : 
+      37             : #include <visualization_msgs/Marker.h>
+      38             : #include <visualization_msgs/MarkerArray.h>
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* defines //{ */
+      43             : 
+      44             : #define MPC_N_STATES 12
+      45             : #define MPC_N_INPUTS 3
+      46             : 
+      47             : #define MPC_HEADING_N_STATES 4
+      48             : #define MPC_HEADING_N_INPUTS 1
+      49             : 
+      50             : #define MPC_HORIZON_LENGTH 40
+      51             : 
+      52             : //}
+      53             : 
+      54             : /* using //{ */
+      55             : 
+      56             : using namespace Eigen;
+      57             : 
+      58             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      59             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      60             : 
+      61             : using radians  = mrs_lib::geometry::radians;
+      62             : using sradians = mrs_lib::geometry::sradians;
+      63             : 
+      64             : using quat_t = Eigen::Quaterniond;
+      65             : 
+      66             : //}
+      67             : 
+      68             : namespace mrs_uav_trackers
+      69             : {
+      70             : 
+      71             : namespace mpc_tracker
+      72             : {
+      73             : 
+      74             : /* //{ class MpcTracker */
+      75             : 
+      76             : class MpcTracker : public mrs_uav_managers::Tracker {
+      77             : public:
+      78             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      79             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      80             : 
+      81             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd);
+      82             :   void                          deactivate(void);
+      83             :   bool                          resetStatic(void);
+      84             : 
+      85             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState& uav_state, const mrs_uav_managers::Controller::ControlOutput& last_control_output);
+      86             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd);
+      87             :   const mrs_msgs::TrackerStatus             getStatus();
+      88             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      89             : 
+      90             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd);
+      91             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd);
+      92             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd);
+      93             : 
+      94             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      95             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      96             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      97             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      98             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      99             : 
+     100             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+     101             : 
+     102             : private:
+     103             :   ros::NodeHandle nh_;
+     104             : 
+     105             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+     106             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+     107             : 
+     108             :   std::atomic<bool> callbacks_enabled_ = true;
+     109             : 
+     110             :   std::string _uav_name_;
+     111             : 
+     112             :   // debugging publishers
+     113             :   mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics> pub_diagnostics_;
+     114             :   mrs_lib::PublisherHandler<std_msgs::String>                pub_status_string_;
+     115             : 
+     116             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_processed_trajectory_poses_;
+     117             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_processed_trajectory_markers_;
+     118             : 
+     119             :   mrs_msgs::UavState uav_state_;
+     120             :   std::mutex         mutex_uav_state_;
+     121             : 
+     122             :   bool is_active_      = false;
+     123             :   bool is_initialized_ = false;
+     124             : 
+     125             :   // | ----------------------- constraints ---------------------- |
+     126             : 
+     127             :   mrs_msgs::DynamicsConstraints constraints_;
+     128             :   std::mutex                    mutex_constraints_;
+     129             : 
+     130             :   mrs_msgs::DynamicsConstraints constraints_filtered_;
+     131             :   std::mutex                    mutex_constraints_filtered_;
+     132             : 
+     133             :   std::atomic<bool> got_constraints_     = false;
+     134             :   std::atomic<bool> all_constraints_set_ = false;
+     135             : 
+     136             :   double _diag_pos_tracking_thr_;
+     137             :   double _diag_heading_tracking_thr_;
+     138             : 
+     139             :   double _mpc_synchronous_rate_limit_;
+     140             :   double _mpc_asynchronous_rate_;
+     141             : 
+     142             :   double update_rate_ = 100.0;
+     143             : 
+     144             :   double     dt1_;
+     145             :   std::mutex mutex_dt1_;
+     146             : 
+     147             :   double _dt2_;
+     148             : 
+     149             :   MatrixXd          _mat_A_;  // system matrix for virtual UAV
+     150             :   MatrixXd          _mat_B_;  // input matrix for virtual UAV
+     151             :   MatrixXd          A_;       // system matrix for virtual UAV
+     152             :   MatrixXd          B_;       // input matrix for virtual UAV
+     153             :   std::atomic<bool> model_first_iteration_ = true;
+     154             :   ros::Time         model_iteration_last_time_;
+     155             : 
+     156             :   MatrixXd _mat_A_heading_;  // system matrix for heading
+     157             :   MatrixXd _mat_B_heading_;  // input matrix for heading
+     158             :   MatrixXd A_heading_;       // system matrix for heading
+     159             :   MatrixXd B_heading_;       // input matrix for heading
+     160             : 
+     161             :   // the reference over the prediction horizon per axis
+     162             :   MatrixXd   des_x_trajectory_;
+     163             :   MatrixXd   des_y_trajectory_;
+     164             :   MatrixXd   des_z_trajectory_;
+     165             :   MatrixXd   des_heading_trajectory_;
+     166             :   std::mutex mutex_des_trajectory_;
+     167             : 
+     168             :   // the reference filtered over the prediction horizon per axis
+     169             :   MatrixXd des_z_filtered_offset_;
+     170             : 
+     171             :   // the whole trajectory reference split per axis
+     172             :   std::shared_ptr<VectorXd> des_x_whole_trajectory_;
+     173             :   std::shared_ptr<VectorXd> des_y_whole_trajectory_;
+     174             :   std::shared_ptr<VectorXd> des_z_whole_trajectory_;
+     175             :   std::shared_ptr<VectorXd> des_heading_whole_trajectory_;
+     176             :   int                       des_whole_trajectory_id_ = 0;
+     177             :   std::mutex                mutex_des_whole_trajectory_;
+     178             : 
+     179             :   int  getCurrentTrajectoryIdx();
+     180             :   void increaseCurrentTrajectoryTime(const double dt);
+     181             : 
+     182             :   // trajectory tracking
+     183             :   std::atomic<bool> trajectory_tracking_in_progress_ = false;
+     184             :   double            trajectory_current_time_;
+     185             :   std::mutex        mutex_trajectory_tracking_states_;
+     186             : 
+     187             :   // params of the loaded trajectory
+     188             :   int    trajectory_size_          = 0;
+     189             :   double trajectory_dt_            = 0.2;
+     190             :   bool   trajectory_track_heading_ = false;
+     191             :   bool   trajectory_tracking_loop_ = false;
+     192             :   bool   trajectory_set_           = false;
+     193             :   int    trajectory_count_         = 0;  // counts how many trajectories we have received
+     194             : 
+     195             :   // mpc output
+     196             :   VectorXd   mpc_u_;
+     197             :   double     mpc_u_heading_;
+     198             :   std::mutex mutex_mpc_u_;
+     199             : 
+     200             :   // current state of the dynamical system
+     201             :   MatrixXd   mpc_x_;          // translation state
+     202             :   MatrixXd   mpc_x_heading_;  // heading state
+     203             :   std::mutex mutex_mpc_x_;
+     204             : 
+     205             :   // odometry reset
+     206             :   std::atomic<bool> odometry_reset_in_progress_ = false;
+     207             :   std::atomic<bool> mpc_result_invalid_         = false;
+     208             : 
+     209             :   // predicting the future
+     210             :   MatrixXd   predicted_trajectory_;
+     211             :   MatrixXd   predicted_heading_trajectory_;
+     212             :   std::mutex mutex_predicted_trajectory_;
+     213             : 
+     214             :   mrs_msgs::MpcPredictionFullState prediction_full_state_;
+     215             :   std::mutex                       mutex_prediction_full_state_;
+     216             : 
+     217             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_predicted_trajectory_debugging_;
+     218             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_mpc_reference_debugging_;
+     219             :   mrs_lib::PublisherHandler<geometry_msgs::PoseStamped> ph_current_trajectory_point_;
+     220             : 
+     221             :   std::atomic<bool> mpc_computed_ = false;
+     222             : 
+     223             :   bool brake_ = false;
+     224             : 
+     225             :   // | ----------------------- MPC solver ----------------------- |
+     226             : 
+     227             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_y_;
+     228             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_x_;
+     229             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_z_;
+     230             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_heading_;
+     231             : 
+     232             :   std::mutex mutex_mpc_calculation_;
+     233             : 
+     234             :   int _max_iters_xy_;
+     235             :   int _max_iters_z_;
+     236             :   int _max_iters_heading_;
+     237             : 
+     238             :   // | ----------- measuring the "MPC realtime factor" ---------- |
+     239             : 
+     240             :   double mpc_rtf_ = 0.0;
+     241             : 
+     242             :   // | ------------------- collision avoidance ------------------ |
+     243             : 
+     244             :   // configurable params
+     245             :   bool collision_avoidance_enabled_           = false;
+     246             :   bool collision_avoidance_enabled_passively_ = true;
+     247             : 
+     248             :   // TODO what is this?
+     249             :   double    coef_scaler = 0;
+     250             :   ros::Time coef_time;
+     251             : 
+     252             :   double minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+     253             : 
+     254             :   // params
+     255             :   double                   _avoidance_trajectory_rate_;
+     256             :   double                   _avoidance_radius_threshold_;
+     257             :   double                   _avoidance_z_correction_;
+     258             :   std::string              _avoidance_diagnostics_topic_name_;
+     259             :   std::vector<std::string> _avoidance_other_uav_names_;
+     260             :   double                   _avoidance_z_threshold_;
+     261             : 
+     262             :   // how old can the other UAV trajectory be (since receive time)
+     263             :   double _collision_trajectory_timeout_;
+     264             : 
+     265             :   // when collision detected, slow down during the manouver
+     266             :   double _avoidance_collision_horizontal_speed_coef_;
+     267             : 
+     268             :   // when collision detected, slow down fully this number of steps before it
+     269             :   int _avoidance_collision_slow_down_fully_;
+     270             : 
+     271             :   // when collision detected, start slowing down this number of steps before it
+     272             :   int _avoidance_collision_slow_down_;
+     273             : 
+     274             :   // when avoiding, start climbing this number of steps before it
+     275             :   int _avoidance_collision_start_climbing_;
+     276             : 
+     277             :   int avoidance_this_uav_number_;
+     278             :   int avoidance_this_uav_priority_;
+     279             : 
+     280             :   double            collision_free_altitude_;
+     281             :   std::atomic<bool> avoiding_collision_               = false;
+     282             :   bool              collision_avoidance_affecting_me_ = false;
+     283             : 
+     284             :   // avoidance trajectory will not be published unless we computed it at least once
+     285             :   std::atomic<bool> future_was_predicted_ = false;
+     286             : 
+     287             :   // subscribing to the other UAV future trajectories
+     288             :   void callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg);
+     289             : 
+     290             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>> other_uav_trajectory_subscribers_;
+     291             :   std::map<std::string, mrs_msgs::FutureTrajectory>                  other_uav_avoidance_trajectories_;
+     292             :   std::mutex                                                         mutex_other_uav_avoidance_trajectories_;
+     293             : 
+     294             :   // subscribing to the other UAV diagnostics'
+     295             :   void callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg);
+     296             : 
+     297             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>> other_uav_diag_subscribers_;
+     298             :   std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>                  other_uav_diagnostics_;
+     299             :   std::mutex                                                              mutex_other_uav_diagnostics_;
+     300             : 
+     301             :   bool checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     302             :   bool checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     303             : 
+     304             :   mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory> ph_avoidance_trajectory_;
+     305             : 
+     306             :   ros::ServiceServer service_server_toggle_avoidance_;
+     307             :   bool               callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     308             : 
+     309             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+     310             : 
+     311             :   // | --------------------- MPC calculation -------------------- |
+     312             : 
+     313             :   ros::Timer        timer_mpc_iteration_;
+     314             :   std::atomic<bool> mpc_synchronous_ = false;
+     315             :   ros::TimerEvent   synchronous_timer_event_;
+     316             : 
+     317             :   std::atomic<bool> mpc_timer_running_ = false;
+     318             :   void              timerMPC(const ros::TimerEvent& event);
+     319             : 
+     320             :   // | -------------------- velocity tracking ------------------- |
+     321             : 
+     322             :   ros::Timer                  timer_velocity_tracking_;
+     323             :   void                        timerVelocityTracking(const ros::TimerEvent& event);
+     324             :   ros::Time                   velocity_reference_time_;
+     325             :   mrs_msgs::VelocityReference velocity_reference_;
+     326             :   std::mutex                  mutex_velocity_reference_;
+     327             :   std::atomic<bool>           velocity_tracking_active_ = false;
+     328             : 
+     329             :   // | ------------------ avoidance trajectory ------------------ |
+     330             : 
+     331             :   ros::Timer timer_avoidance_trajectory_;
+     332             :   void       timerAvoidanceTrajectory(const ros::TimerEvent& event);
+     333             : 
+     334             :   // | ----------------------- diagnostics ---------------------- |
+     335             : 
+     336             :   ros::Timer timer_diagnostics_;
+     337             :   double     _diagnostics_rate_;
+     338             :   void       timerDiagnostics(const ros::TimerEvent& event);
+     339             : 
+     340             :   // | ------------------------ hovering ------------------------ |
+     341             : 
+     342             :   ros::Timer        timer_hover_;
+     343             :   void              timerHover(const ros::TimerEvent& event);
+     344             :   std::atomic<bool> hovering_in_progress_ = false;
+     345             :   void              toggleHover(bool in);
+     346             : 
+     347             :   // | ------------------- trajectory tracking ------------------ |
+     348             : 
+     349             :   std::tuple<bool, std::string> resumeTrajectoryTrackingImpl(void);
+     350             :   std::tuple<bool, std::string> startTrajectoryTrackingImpl(void);
+     351             :   std::tuple<bool, std::string> stopTrajectoryTrackingImpl(void);
+     352             :   std::tuple<bool, std::string> gotoTrajectoryStartImpl(void);
+     353             : 
+     354             :   // | --------------------- other routines --------------------- |
+     355             : 
+     356             :   void publishDiagnostics();
+     357             : 
+     358             :   void debugPrintState(const double throttle);
+     359             :   void debugPrintMPCResult(const double throttle);
+     360             : 
+     361             :   void setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     362             :   void setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     363             :   void setSinglePointReference(const double x, const double y, const double z, const double heading);
+     364             : 
+     365             :   std::tuple<bool, std::string, bool> loadTrajectory(const mrs_msgs::TrajectoryReference msg);
+     366             : 
+     367             :   MatrixXd                       filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed);
+     368             :   std::tuple<MatrixXd, MatrixXd> filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x, double max_speed_y);
+     369             : 
+     370             :   double checkTrajectoryForCollisions(int& first_collision_index);
+     371             : 
+     372             :   void manageConstraints(void);
+     373             :   void calculateMPC(void);
+     374             :   void iterateModel(const double& dt);
+     375             : 
+     376             :   // | ------------------------ profiler ------------------------ |
+     377             : 
+     378             :   mrs_lib::Profiler profiler;
+     379             :   bool              _profiler_enabled_ = false;
+     380             : 
+     381             :   // | ------------------------- wiggle ------------------------- |
+     382             : 
+     383             :   ros::ServiceServer service_server_wiggle_;
+     384             :   bool               callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     385             : 
+     386             :   double wiggle_phase_ = 0;
+     387             : 
+     388             :   // | --------------- dynamic reconfigure server --------------- |
+     389             : 
+     390             :   void dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, uint32_t level);
+     391             : 
+     392             :   boost::recursive_mutex                      config_mutex_;
+     393             :   typedef mrs_uav_trackers::mpc_trackerConfig Config;
+     394             :   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+     395             :   boost::shared_ptr<ReconfigureServer>        reconfigure_server_;
+     396             :   mrs_uav_trackers::mpc_trackerConfig         drs_params_;
+     397             :   std::mutex                                  mutex_drs_params_;
+     398             : };
+     399             : 
+     400             : //}
+     401             : 
+     402             : // | -------------- tracker's interface routines -------------- |
+     403             : 
+     404             : /* //{ initialize() */
+     405             : 
+     406         107 : bool MpcTracker::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     407             :                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     408             : 
+     409         107 :   nh_ = nh;
+     410             : 
+     411         107 :   common_handlers_  = common_handlers;
+     412         107 :   private_handlers_ = private_handlers;
+     413             : 
+     414         107 :   _uav_name_ = common_handlers->uav_name;
+     415             : 
+     416         107 :   ros::Time::waitForValid();
+     417             : 
+     418             :   // --------------------------------------------------------------
+     419             :   // |                     loading parameters                     |
+     420             :   // --------------------------------------------------------------
+     421             : 
+     422             :   // | ---------- loading params using the parent's nh ---------- |
+     423             : 
+     424         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     425             : 
+     426         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     427             : 
+     428         107 :   if (!param_loader_parent.loadedSuccessfully()) {
+     429           0 :     ROS_ERROR("[MpcTracker]: Could not load all parameters!");
+     430           0 :     return false;
+     431             :   }
+     432             : 
+     433             :   // | --------------- loading plugin's parameters -------------- |
+     434             : 
+     435         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/mpc_tracker.yaml");
+     436         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/mpc_tracker.yaml");
+     437             : 
+     438         214 :   const std::string yaml_prefix = "mrs_uav_trackers/mpc_tracker/";
+     439             : 
+     440         107 :   private_handlers->param_loader->loadParam("network/robot_names", _avoidance_other_uav_names_);
+     441             : 
+     442         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/synchronous_rate_limit", _mpc_synchronous_rate_limit_);
+     443         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/asynchronous_loop_rate", _mpc_asynchronous_rate_);
+     444             : 
+     445         107 :   if (_mpc_asynchronous_rate_ < 15) {
+     446           0 :     ROS_ERROR("[MpcTracker]: the asynchronous_loop_rate must be > 15 Hz");
+     447           0 :     return false;
+     448             :   }
+     449             : 
+     450         107 :   dt1_ = 1.0 / _mpc_asynchronous_rate_;
+     451             : 
+     452         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/enabled", drs_params_.braking_enabled);
+     453         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_braking", drs_params_.q_vel_braking);
+     454         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_no_braking", drs_params_.q_vel_no_braking);
+     455             : 
+     456         107 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/A", _mat_A_, MPC_N_STATES, MPC_N_STATES);
+     457         107 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/B", _mat_B_, MPC_N_STATES, MPC_N_INPUTS);
+     458             : 
+     459         107 :   A_ = _mat_A_;
+     460         107 :   B_ = _mat_B_;
+     461             : 
+     462         107 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/A", _mat_A_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_STATES);
+     463         107 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/B", _mat_B_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_INPUTS);
+     464             : 
+     465         107 :   A_heading_ = _mat_A_heading_;
+     466         107 :   B_heading_ = _mat_B_heading_;
+     467             : 
+     468             :   // load the MPC parameters
+     469         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/dt2", _dt2_);
+     470             : 
+     471         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_rate_);
+     472         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/position_tracking_threshold", _diag_pos_tracking_thr_);
+     473         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/orientation_tracking_threshold", _diag_heading_tracking_thr_);
+     474             : 
+     475         107 :   bool verbose_xy      = false;
+     476         107 :   bool verbose_z       = false;
+     477         107 :   bool verbose_heading = false;
+     478             : 
+     479         214 :   std::vector<double> xy_Q;
+     480         214 :   std::vector<double> z_Q;
+     481         214 :   std::vector<double> heading_Q;
+     482             : 
+     483         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/verbose", verbose_xy);
+     484         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/max_n_iterations", _max_iters_xy_);
+     485         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/Q", xy_Q);
+     486             : 
+     487         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/verbose", verbose_z);
+     488         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/max_n_iterations", _max_iters_z_);
+     489         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/Q", z_Q);
+     490             : 
+     491         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/verbose", verbose_heading);
+     492         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/max_n_iterations", _max_iters_heading_);
+     493         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/Q", heading_Q);
+     494             : 
+     495         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/enabled", drs_params_.wiggle_enabled);
+     496         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/amplitude", drs_params_.wiggle_amplitude);
+     497         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/frequency", drs_params_.wiggle_frequency);
+     498             : 
+     499         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled", collision_avoidance_enabled_);
+     500         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled_passively", collision_avoidance_enabled_passively_);
+     501         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/predicted_trajectory_publish_rate", _avoidance_trajectory_rate_);
+     502         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/correction", _avoidance_z_correction_);
+     503         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/radius", _avoidance_radius_threshold_);
+     504         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/altitude_threshold", _avoidance_z_threshold_);
+     505         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_horizontal_speed_coef", _avoidance_collision_horizontal_speed_coef_);
+     506         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_fully", _avoidance_collision_slow_down_fully_);
+     507         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_start", _avoidance_collision_slow_down_);
+     508         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_start_climbing", _avoidance_collision_start_climbing_);
+     509         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/trajectory_timeout", _collision_trajectory_timeout_);
+     510             : 
+     511         107 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     512           0 :     ROS_ERROR("[MpcTracker]: could not load all parameters!");
+     513           0 :     return false;
+     514             :   }
+     515             : 
+     516         107 :   ROS_INFO_STREAM("[MpcTracker]: initializing solvers with dt1 = " << dt1_);
+     517             : 
+     518         107 :   mpc_solver_y_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_y", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 1);
+     519         107 :   mpc_solver_x_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_x", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 0);
+     520         107 :   mpc_solver_z_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_z", verbose_z, _max_iters_z_, z_Q, dt1_, _dt2_, 2);
+     521             :   mpc_solver_heading_ =
+     522         107 :       std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_hdg", verbose_heading, _max_iters_heading_, heading_Q, dt1_, _dt2_, 0);
+     523             : 
+     524         107 :   mpc_x_         = MatrixXd::Zero(MPC_N_STATES, 1);
+     525         107 :   mpc_x_heading_ = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     526             : 
+     527         107 :   mpc_u_ = VectorXd::Zero(MPC_N_INPUTS);
+     528             : 
+     529         107 :   coef_time = ros::Time(0);
+     530             : 
+     531         107 :   des_x_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     532         107 :   des_y_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     533         107 :   des_z_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     534         107 :   des_z_filtered_offset_  = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     535         107 :   des_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     536             : 
+     537         107 :   service_server_wiggle_ = nh_.advertiseService("wiggle", &MpcTracker::callbackWiggle, this);
+     538             : 
+     539         107 :   pub_diagnostics_   = mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics>(nh_, "diagnostics", 1);
+     540         107 :   pub_status_string_ = mrs_lib::PublisherHandler<std_msgs::String>(nh_, "string", 1);
+     541             : 
+     542             :   // extract the numerical name
+     543         107 :   sscanf(_uav_name_.c_str(), "uav%d", &avoidance_this_uav_number_);
+     544         107 :   ROS_INFO("[MpcTracker]: Numerical ID of this UAV is %d", avoidance_this_uav_number_);
+     545         107 :   avoidance_this_uav_priority_ = avoidance_this_uav_number_;
+     546             : 
+     547             :   // exclude this drone from the list
+     548         107 :   std::vector<std::string>::iterator it = _avoidance_other_uav_names_.begin();
+     549         224 :   while (it != _avoidance_other_uav_names_.end()) {
+     550             : 
+     551         117 :     std::string temp_str = *it;
+     552             : 
+     553             :     int other_uav_priority;
+     554         117 :     sscanf(temp_str.c_str(), "uav%d", &other_uav_priority);
+     555             : 
+     556         117 :     if (other_uav_priority == avoidance_this_uav_number_) {
+     557             : 
+     558         107 :       _avoidance_other_uav_names_.erase(it);
+     559         107 :       continue;
+     560             :     }
+     561             : 
+     562          10 :     it++;
+     563             :   }
+     564             : 
+     565             :   // initialize velocity tracker
+     566             : 
+     567         107 :   velocity_reference_time_ = ros::Time(0);
+     568             : 
+     569             :   // create publishers for predicted trajectory
+     570             : 
+     571         107 :   ph_avoidance_trajectory_           = mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory>(nh_, "predicted_trajectory", 1);
+     572         107 :   ph_predicted_trajectory_debugging_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "predicted_trajectory_debugging", 1);
+     573         107 :   ph_mpc_reference_debugging_        = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "mpc_reference_debugging", 1, true);
+     574         107 :   ph_current_trajectory_point_       = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "current_trajectory_point", 1, true);
+     575             : 
+     576         107 :   pub_debug_processed_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_processed/poses", 1, true);
+     577         107 :   pub_debug_processed_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_processed/markers", 1, true);
+     578             : 
+     579             :   // preallocate predicted trajectory
+     580         107 :   predicted_trajectory_         = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     581         107 :   predicted_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     582             : 
+     583         107 :   collision_free_altitude_ = std::numeric_limits<float>::lowest();
+     584             : 
+     585             :   // collision avoidance toggle service
+     586         107 :   service_server_toggle_avoidance_ = nh_.advertiseService("collision_avoidance", &MpcTracker::callbackToggleCollisionAvoidance, this);
+     587             : 
+     588         214 :   mrs_lib::SubscribeHandlerOptions shopts;
+     589         107 :   shopts.nh                 = nh_;
+     590         107 :   shopts.node_name          = "MpcTracker";
+     591         107 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     592         107 :   shopts.threadsafe         = true;
+     593         107 :   shopts.autostart          = true;
+     594         107 :   shopts.queue_size         = 10;
+     595         107 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     596             : 
+     597             :   // create subscribers on other drones diagnostics
+     598         107 :   if (collision_avoidance_enabled_ || collision_avoidance_enabled_passively_) {
+     599             : 
+     600         117 :     for (int i = 0; i < int(_avoidance_other_uav_names_.size()); i++) {
+     601             : 
+     602          30 :       std::string prediction_topic_name = std::string("/") + _avoidance_other_uav_names_.at(i) + "/control_manager/mpc_tracker/predicted_trajectory";
+     603          20 :       std::string diag_topic_name       = std::string("/") + _avoidance_other_uav_names_.at(i) + "/control_manager/mpc_tracker/diagnostics";
+     604             : 
+     605          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", prediction_topic_name.c_str());
+     606             : 
+     607          10 :       other_uav_trajectory_subscribers_.push_back(
+     608          20 :           mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>(shopts, prediction_topic_name, &MpcTracker::callbackOtherMavTrajectory, this));
+     609             : 
+     610          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", diag_topic_name.c_str());
+     611             : 
+     612          10 :       other_uav_diag_subscribers_.push_back(
+     613          20 :           mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>(shopts, diag_topic_name, &MpcTracker::callbackOtherMavDiagnostics, this));
+     614             :     }
+     615             :   }
+     616             : 
+     617         107 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, std::string("/") + _uav_name_ + "/estimation_manager/diagnostics");
+     618             : 
+     619             :   // | --------------- dynamic reconfigure server --------------- |
+     620             : 
+     621         107 :   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, nh_));
+     622         107 :   reconfigure_server_->updateConfig(drs_params_);
+     623         107 :   ReconfigureServer::CallbackType f = boost::bind(&MpcTracker::dynamicReconfigureCallback, this, _1, _2);
+     624         107 :   reconfigure_server_->setCallback(f);
+     625             : 
+     626             :   // | ------------------------ profiler ------------------------ |
+     627             : 
+     628         107 :   profiler = mrs_lib::Profiler(common_handlers->parent_nh, "MpcTracker", _profiler_enabled_);
+     629             : 
+     630             :   // | ------------------------- timers ------------------------- |
+     631             : 
+     632         214 :   timer_avoidance_trajectory_ = nh_.createTimer(ros::Rate(_avoidance_trajectory_rate_), &MpcTracker::timerAvoidanceTrajectory, this, false,
+     633         214 :                                                 collision_avoidance_enabled_ || collision_avoidance_enabled_passively_);
+     634         107 :   timer_diagnostics_          = nh_.createTimer(ros::Rate(_diagnostics_rate_), &MpcTracker::timerDiagnostics, this);
+     635         107 :   timer_mpc_iteration_        = nh_.createTimer(ros::Rate(_mpc_asynchronous_rate_), &MpcTracker::timerMPC, this, false, false);
+     636         107 :   timer_velocity_tracking_    = nh_.createTimer(ros::Rate(30.0), &MpcTracker::timerVelocityTracking, this, false, false);
+     637         107 :   timer_hover_                = nh_.createTimer(ros::Rate(10.0), &MpcTracker::timerHover, this, false, false);
+     638             : 
+     639             :   // | ----------------------- finish init ---------------------- |
+     640             : 
+     641         107 :   is_initialized_ = true;
+     642             : 
+     643         107 :   ROS_INFO("[MpcTracker]: initialized");
+     644             : 
+     645         107 :   return true;
+     646             : }
+     647             : 
+     648             : //}
+     649             : 
+     650             : /* //{ activate() */
+     651             : 
+     652          98 : std::tuple<bool, std::string> MpcTracker::activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     653             : 
+     654         196 :   std::stringstream ss;
+     655             : 
+     656          98 :   if (!got_constraints_) {
+     657             : 
+     658           0 :     ss << "can not activate, missing constraints";
+     659           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     660             : 
+     661           0 :     return std::tuple(false, ss.str());
+     662             :   }
+     663             : 
+     664         196 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     665             : 
+     666             :   double uav_state_heading;
+     667             : 
+     668             :   try {
+     669          98 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     670             :   }
+     671           0 :   catch (...) {
+     672           0 :     ss << "could not calculate the UAV heading";
+     673           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     674           0 :     return std::tuple(false, ss.str());
+     675             :   }
+     676             : 
+     677         196 :   MatrixXd mpc_x         = MatrixXd::Zero(MPC_N_STATES, 1);
+     678          98 :   MatrixXd mpc_x_heading = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     679             : 
+     680          98 :   if (last_tracker_cmd) {
+     681             : 
+     682             :     // set the initial condition from the last tracker's cmd
+     683             : 
+     684          95 :     if (last_tracker_cmd->use_position_horizontal) {
+     685          95 :       mpc_x(0, 0) = last_tracker_cmd->position.x;
+     686          95 :       mpc_x(4, 0) = last_tracker_cmd->position.y;
+     687             :     } else {
+     688           0 :       mpc_x(0, 0) = uav_state.pose.position.x;
+     689           0 :       mpc_x(4, 0) = uav_state.pose.position.y;
+     690             :     }
+     691             : 
+     692          95 :     if (last_tracker_cmd->use_position_vertical) {
+     693          95 :       mpc_x(8, 0) = last_tracker_cmd->position.z;
+     694             :     } else {
+     695           0 :       mpc_x(8, 0) = uav_state.pose.position.z;
+     696             :     }
+     697             : 
+     698          95 :     if (last_tracker_cmd->use_velocity_horizontal) {
+     699          95 :       mpc_x(1, 0) = last_tracker_cmd->velocity.x;
+     700          95 :       mpc_x(5, 0) = last_tracker_cmd->velocity.y;
+     701             :     } else {
+     702           0 :       mpc_x(1, 0) = uav_state.velocity.linear.x;
+     703           0 :       mpc_x(5, 0) = uav_state.velocity.linear.y;
+     704             :     }
+     705             : 
+     706          95 :     if (last_tracker_cmd->use_velocity_vertical) {
+     707          95 :       mpc_x(9, 0) = last_tracker_cmd->velocity.z;
+     708             :     } else {
+     709           0 :       mpc_x(9, 0) = uav_state.velocity.linear.z;
+     710             :     }
+     711             : 
+     712          95 :     if (last_tracker_cmd->use_acceleration) {
+     713           0 :       mpc_x(2, 0)  = last_tracker_cmd->acceleration.x;
+     714           0 :       mpc_x(6, 0)  = last_tracker_cmd->acceleration.y;
+     715           0 :       mpc_x(10, 0) = last_tracker_cmd->acceleration.z;
+     716             :     } else {
+     717          95 :       mpc_x(2, 0)  = 0;
+     718          95 :       mpc_x(6, 0)  = 0;
+     719          95 :       mpc_x(10, 0) = 0;
+     720             :     }
+     721             : 
+     722             :     // the jerks
+     723          95 :     mpc_x(3, 0)  = 0;
+     724          95 :     mpc_x(7, 0)  = 0;
+     725          95 :     mpc_x(11, 0) = 0;
+     726             : 
+     727          95 :     if (last_tracker_cmd->use_heading) {
+     728          95 :       mpc_x_heading(0, 0) = last_tracker_cmd->heading;
+     729           0 :     } else if (last_tracker_cmd->use_orientation) {
+     730             :       try {
+     731           0 :         mpc_x_heading(0, 0) = mrs_lib::AttitudeConverter(last_tracker_cmd->orientation).getHeading();
+     732             :       }
+     733           0 :       catch (...) {
+     734           0 :         mpc_x_heading(0, 0) = uav_state_heading;
+     735             :       }
+     736             :     } else {
+     737           0 :       mpc_x_heading(0, 0) = uav_state_heading;
+     738             :     }
+     739             : 
+     740          95 :     if (last_tracker_cmd->use_heading_rate) {
+     741          20 :       mpc_x_heading(1, 0) = last_tracker_cmd->heading_rate;
+     742             :     } else {
+     743          75 :       mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     744             :     }
+     745             : 
+     746          95 :     mpc_x_heading(2, 0) = 0;
+     747          95 :     mpc_x_heading(3, 0) = 0;
+     748             : 
+     749          95 :     ROS_INFO("[MpcTracker]: activated with last tracker's command");
+     750             : 
+     751             :   } else {
+     752             : 
+     753             :     // set the initial condition completely from the uav_state
+     754             : 
+     755           3 :     mpc_x(0, 0) = uav_state.pose.position.x;
+     756           3 :     mpc_x(1, 0) = uav_state.velocity.linear.x;
+     757           3 :     mpc_x(2, 0) = 0;
+     758           3 :     mpc_x(3, 0) = 0;
+     759             : 
+     760           3 :     mpc_x(4, 0) = uav_state.pose.position.y;
+     761           3 :     mpc_x(5, 0) = uav_state.velocity.linear.y;
+     762           3 :     mpc_x(6, 0) = 0;
+     763           3 :     mpc_x(7, 0) = 0;
+     764             : 
+     765           3 :     mpc_x(8, 0)  = uav_state.pose.position.z;
+     766           3 :     mpc_x(9, 0)  = uav_state.velocity.linear.z;
+     767           3 :     mpc_x(10, 0) = 0;
+     768           3 :     mpc_x(11, 0) = 0;
+     769             : 
+     770           3 :     mpc_x_heading(0, 0) = uav_state_heading;
+     771           3 :     mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     772           3 :     mpc_x_heading(2, 0) = 0;
+     773           3 :     mpc_x_heading(3, 0) = 0;
+     774             : 
+     775           3 :     ROS_INFO("[MpcTracker]: activated with uav state");
+     776             :   }
+     777             : 
+     778             :   {
+     779         196 :     std::scoped_lock lock(mutex_mpc_x_);
+     780             : 
+     781          98 :     mpc_x_         = mpc_x;
+     782          98 :     mpc_x_heading_ = mpc_x_heading;
+     783             :   }
+     784             : 
+     785          98 :   trajectory_tracking_in_progress_ = false;
+     786             : 
+     787          98 :   ss << "activated";
+     788          98 :   ROS_INFO_STREAM("[MpcTracker]: " << ss.str());
+     789             : 
+     790             :   // this is here to initialize the desired_trajectory vector
+     791             :   // if deleted (and I tried) the UAV will briefly fly to the
+     792             :   // origin after activation
+     793          98 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     794             : 
+     795          98 :   toggleHover(true);
+     796             : 
+     797          98 :   model_first_iteration_ = true;
+     798             : 
+     799          98 :   A_ = _mat_A_;
+     800          98 :   B_ = _mat_B_;
+     801             : 
+     802          98 :   A_heading_ = _mat_A_heading_;
+     803          98 :   B_heading_ = _mat_B_heading_;
+     804             : 
+     805          98 :   is_active_ = true;
+     806             : 
+     807          98 :   if (!mpc_synchronous_) {
+     808          88 :     timer_mpc_iteration_.start();
+     809             :   }
+     810             : 
+     811          98 :   return std::tuple(true, ss.str());
+     812             : }
+     813             : 
+     814             : //}
+     815             : 
+     816             : /* //{ deactivate() */
+     817             : 
+     818          40 : void MpcTracker::deactivate(void) {
+     819             : 
+     820          40 :   toggleHover(false);
+     821             : 
+     822          40 :   is_active_                       = false;
+     823          40 :   trajectory_tracking_in_progress_ = false;
+     824          40 :   model_first_iteration_           = true;
+     825             : 
+     826             :   {
+     827          40 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+     828             : 
+     829          40 :     trajectory_current_time_ = 0;
+     830             :   }
+     831             : 
+     832          40 :   ROS_INFO("[MpcTracker]: deactivated");
+     833             : 
+     834          40 :   timer_mpc_iteration_.stop();
+     835             : 
+     836          40 :   publishDiagnostics();
+     837          40 : }
+     838             : 
+     839             : //}
+     840             : 
+     841             : /* //{ resetStatic() */
+     842             : 
+     843           0 : bool MpcTracker::resetStatic(void) {
+     844             : 
+     845           0 :   if (!is_initialized_) {
+     846           0 :     ROS_ERROR("[MpcTracker]: can not reset, not initialized");
+     847           0 :     return false;
+     848             :   }
+     849             : 
+     850           0 :   if (!is_active_) {
+     851           0 :     ROS_ERROR("[MpcTracker]: can not reset, not active");
+     852           0 :     return false;
+     853             :   }
+     854             : 
+     855           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     856             : 
+     857             :   double uav_state_heading;
+     858             : 
+     859             :   try {
+     860           0 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     861             :   }
+     862           0 :   catch (...) {
+     863           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: could not calculate the UAV heading");
+     864           0 :     return false;
+     865             :   }
+     866             : 
+     867             :   {
+     868           0 :     std::scoped_lock lock(mutex_mpc_x_);
+     869             : 
+     870             :     // set the initial condition from the odometry
+     871             : 
+     872           0 :     ROS_INFO("[MpcTracker]: reseting with uav state with no dynamics");
+     873             : 
+     874           0 :     mpc_x_(0, 0) = uav_state.pose.position.x;
+     875           0 :     mpc_x_(1, 0) = 0;
+     876           0 :     mpc_x_(2, 0) = 0;
+     877           0 :     mpc_x_(3, 0) = 0;
+     878             : 
+     879           0 :     mpc_x_(4, 0) = uav_state.pose.position.y;
+     880           0 :     mpc_x_(5, 0) = 0;
+     881           0 :     mpc_x_(6, 0) = 0;
+     882           0 :     mpc_x_(7, 0) = 0;
+     883             : 
+     884           0 :     mpc_x_(8, 0)  = uav_state.pose.position.z;
+     885           0 :     mpc_x_(9, 0)  = 0;
+     886           0 :     mpc_x_(10, 0) = 0;
+     887           0 :     mpc_x_(11, 0) = 0;
+     888             : 
+     889           0 :     mpc_x_heading_(0, 0) = uav_state_heading;
+     890           0 :     mpc_x_heading_(1, 0) = 0;
+     891           0 :     mpc_x_heading_(2, 0) = 0;
+     892           0 :     mpc_x_heading_(3, 0) = 0;
+     893             : 
+     894           0 :     trajectory_tracking_in_progress_ = false;
+     895             : 
+     896           0 :     ROS_INFO("[MpcTracker]: reseted");
+     897             :   }
+     898             : 
+     899             :   // this is here to initialize the desired_trajectory vector
+     900             :   // if deleted (and I tried) the UAV will briefly fly to the
+     901             :   // origin after activation
+     902           0 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     903             : 
+     904           0 :   return true;
+     905             : }
+     906             : 
+     907             : //}
+     908             : 
+     909             : /* //{ update() */
+     910             : 
+     911      136027 : std::optional<mrs_msgs::TrackerCommand> MpcTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     912             :                                                            [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     913             : 
+     914      408081 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("update");
+     915      408081 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("MpcTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     916             : 
+     917      272054 :   auto old_uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     918             : 
+     919             :   // the time from the last update call
+     920      136027 :   double dt = (uav_state.header.stamp - old_uav_state.header.stamp).toSec();
+     921             : 
+     922             :   // save the uav state
+     923      136027 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     924             : 
+     925      136027 :   if (dt > 0) {
+     926             : 
+     927      136027 :     double rate = 1.0 / dt;
+     928             : 
+     929      136027 :     update_rate_ = 0.9 * update_rate_ + 0.1 * rate;
+     930             : 
+     931      136027 :     if (mpc_synchronous_ && (update_rate_ > _mpc_synchronous_rate_limit_)) {
+     932           7 :       mpc_synchronous_ = false;
+     933           7 :       ROS_INFO("[MpcTracker]: detecting high update date (%.1f Hz > %.1f Hz), switching to asynchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     934           7 :       if (is_active_) {
+     935           7 :         timer_mpc_iteration_.start();
+     936             :       }
+     937      136020 :     } else if (!mpc_synchronous_ && (update_rate_ <= _mpc_synchronous_rate_limit_)) {
+     938          21 :       mpc_synchronous_ = true;
+     939          21 :       ROS_INFO("[MpcTracker]: detecting low update rate (%.1f Hz < %.1f Hz), switching to synchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     940          21 :       timer_mpc_iteration_.stop();
+     941             :     }
+     942             :   }
+     943             : 
+     944             :   // up to this part the update() method is evaluated even when the tracker is not active
+     945      136027 :   if (!is_active_) {
+     946       55960 :     return {};
+     947             :   }
+     948             : 
+     949      160134 :   mrs_msgs::TrackerCommand tracker_cmd;
+     950             : 
+     951       80067 :   if (!mpc_synchronous_ && (!mpc_computed_ || mpc_result_invalid_)) {
+     952             : 
+     953         138 :     ROS_WARN_THROTTLE(0.1, "[MpcTracker]: MPC not ready, returning current odom as the command");
+     954             : 
+     955             :     // set the header
+     956         138 :     tracker_cmd.header.stamp    = uav_state.header.stamp;
+     957         138 :     tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     958             : 
+     959             :     // set positions from odom
+     960         138 :     tracker_cmd.position.x              = uav_state.pose.position.x;
+     961         138 :     tracker_cmd.position.y              = uav_state.pose.position.y;
+     962         138 :     tracker_cmd.position.z              = uav_state.pose.position.z;
+     963         138 :     tracker_cmd.use_position_vertical   = 1;
+     964         138 :     tracker_cmd.use_position_horizontal = 1;
+     965             : 
+     966             :     // set velocities from odom
+     967         138 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     968         138 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     969         138 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     970         138 :     tracker_cmd.use_velocity_vertical   = 1;
+     971         138 :     tracker_cmd.use_velocity_horizontal = 1;
+     972             : 
+     973             :     // set zero accelerations
+     974         138 :     tracker_cmd.acceleration.x   = 0;
+     975         138 :     tracker_cmd.acceleration.y   = 0;
+     976         138 :     tracker_cmd.acceleration.z   = 0;
+     977         138 :     tracker_cmd.use_acceleration = 1;
+     978             : 
+     979             :     try {
+     980         138 :       tracker_cmd.heading     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     981         138 :       tracker_cmd.use_heading = 1;
+     982             :     }
+     983           0 :     catch (...) {
+     984           0 :       tracker_cmd.use_heading = 0;
+     985           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading");
+     986             :     }
+     987             : 
+     988             :     // set zero jerk
+     989         138 :     tracker_cmd.jerk.x = 0;
+     990         138 :     tracker_cmd.jerk.y = 0;
+     991         138 :     tracker_cmd.jerk.z = 0;
+     992             : 
+     993             :     try {
+     994         138 :       tracker_cmd.heading_rate     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(uav_state.velocity.angular);
+     995         138 :       tracker_cmd.use_heading_rate = 1;
+     996             :     }
+     997           0 :     catch (...) {
+     998           0 :       tracker_cmd.use_heading_rate = 0;
+     999           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading rate");
+    1000             :     }
+    1001             : 
+    1002         138 :     return {tracker_cmd};
+    1003             :   }
+    1004             : 
+    1005       79929 :   if (mpc_synchronous_) {
+    1006             : 
+    1007        4022 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
+    1008             : 
+    1009        4022 :     if (synchronous_timer_event_.last_real.toSec() > 0) {
+    1010        4007 :       synchronous_timer_event_.last_real = synchronous_timer_event_.current_real;
+    1011             :     } else {
+    1012          15 :       synchronous_timer_event_.last_real = ros::Time::now();
+    1013             :     }
+    1014             : 
+    1015        4022 :     synchronous_timer_event_.current_real = ros::Time::now();
+    1016             : 
+    1017        4022 :     timerMPC(synchronous_timer_event_);
+    1018             : 
+    1019             :   } else {
+    1020       75907 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
+    1021             :   }
+    1022             : 
+    1023       79929 :   if (dt > 0) {
+    1024       79929 :     iterateModel(dt);
+    1025             :   } else {
+    1026           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: dt !> 0, not iterating the model");
+    1027             :   }
+    1028             : 
+    1029      159858 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1030      159858 :   auto prediction_full_state  = mrs_lib::get_mutexed(mutex_prediction_full_state_, prediction_full_state_);
+    1031             : 
+    1032             :   // check whether all outputs are finite
+    1033       79929 :   bool arefinite = true;
+    1034     1039077 :   for (int i = 0; i < 12; i++) {
+    1035      959148 :     if (!std::isfinite(mpc_x(i, 0))) {
+    1036           0 :       arefinite = false;
+    1037             :     }
+    1038             :   }
+    1039             : 
+    1040       79929 :   if (arefinite) {
+    1041             : 
+    1042             :     // set the desired states base on the result of the mpc
+    1043       79929 :     tracker_cmd.position.x     = mpc_x(0, 0);
+    1044       79929 :     tracker_cmd.velocity.x     = mpc_x(1, 0);
+    1045       79929 :     tracker_cmd.acceleration.x = mpc_x(2, 0);
+    1046       79929 :     tracker_cmd.jerk.x         = mpc_x(3, 0);
+    1047             : 
+    1048       79929 :     tracker_cmd.position.y     = mpc_x(4, 0);
+    1049       79929 :     tracker_cmd.velocity.y     = mpc_x(5, 0);
+    1050       79929 :     tracker_cmd.acceleration.y = mpc_x(6, 0);
+    1051       79929 :     tracker_cmd.jerk.y         = mpc_x(7, 0);
+    1052             : 
+    1053       79929 :     tracker_cmd.position.z     = mpc_x(8, 0);
+    1054       79929 :     tracker_cmd.velocity.z     = mpc_x(9, 0);
+    1055       79929 :     tracker_cmd.acceleration.z = mpc_x(10, 0);
+    1056       79929 :     tracker_cmd.jerk.z         = mpc_x(11, 0);
+    1057             : 
+    1058       79929 :     tracker_cmd.full_state_prediction = prediction_full_state;
+    1059             : 
+    1060       79929 :     tracker_cmd.use_position_vertical     = 1;
+    1061       79929 :     tracker_cmd.use_position_horizontal   = 1;
+    1062       79929 :     tracker_cmd.use_velocity_vertical     = 1;
+    1063       79929 :     tracker_cmd.use_velocity_horizontal   = 1;
+    1064       79929 :     tracker_cmd.use_acceleration          = 1;
+    1065       79929 :     tracker_cmd.use_jerk                  = 1;
+    1066       79929 :     tracker_cmd.use_full_state_prediction = 1;
+    1067             : 
+    1068             :   } else {
+    1069             : 
+    1070           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC translation outputs are not finite!");
+    1071             : 
+    1072           0 :     return {};
+    1073             :   }
+    1074             : 
+    1075       79929 :   bool heading_finite = true;
+    1076      399645 :   for (int i = 0; i < MPC_HEADING_N_STATES; i++) {
+    1077      319716 :     if (!std::isfinite(mpc_x_heading(i, 0))) {
+    1078           0 :       heading_finite = false;
+    1079             :     }
+    1080             :   }
+    1081             : 
+    1082       79929 :   if (heading_finite) {
+    1083             : 
+    1084       79929 :     tracker_cmd.heading              = mpc_x_heading(0, 0);
+    1085       79929 :     tracker_cmd.heading_rate         = mpc_x_heading(1, 0);
+    1086       79929 :     tracker_cmd.heading_acceleration = mpc_x_heading(2, 0);
+    1087       79929 :     tracker_cmd.heading_jerk         = mpc_x_heading(3, 0);
+    1088             : 
+    1089       79929 :     tracker_cmd.use_heading              = 1;
+    1090       79929 :     tracker_cmd.use_heading_rate         = 1;
+    1091       79929 :     tracker_cmd.use_heading_acceleration = 1;
+    1092       79929 :     tracker_cmd.use_heading_jerk         = 1;
+    1093             : 
+    1094             :   } else {
+    1095             : 
+    1096           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC heading output is not finite!");
+    1097             : 
+    1098           0 :     return {};
+    1099             :   }
+    1100             : 
+    1101             :   // set the header
+    1102       79929 :   tracker_cmd.header.stamp    = uav_state.header.stamp;
+    1103       79929 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+    1104             : 
+    1105             :   // u have to return a position command
+    1106             :   // can set the jerk to 0
+    1107       79929 :   return {tracker_cmd};
+    1108             : }  // namespace mpc_tracker
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ getStatus() */
+    1113             : 
+    1114        9231 : const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
+    1115             : 
+    1116       18462 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1117        9231 :   auto trajectory_size        = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    1118             : 
+    1119             :   double des_x, des_y, des_z, des_heading;
+    1120             :   {
+    1121        9231 :     std::scoped_lock lock(mutex_des_trajectory_);
+    1122             : 
+    1123        9231 :     des_x       = des_x_trajectory_(0);
+    1124        9231 :     des_y       = des_y_trajectory_(0);
+    1125        9231 :     des_z       = des_z_trajectory_(0);
+    1126        9231 :     des_heading = des_heading_trajectory_(0);
+    1127             :   }
+    1128             : 
+    1129        9231 :   mrs_msgs::TrackerStatus tracker_status;
+    1130             : 
+    1131        9231 :   tracker_status.active            = is_active_;
+    1132        9231 :   tracker_status.callbacks_enabled = is_active_ && callbacks_enabled_ && !hovering_in_progress_;
+    1133             : 
+    1134        9231 :   tracker_status.tracking_trajectory = trajectory_tracking_in_progress_;
+    1135             : 
+    1136        9231 :   bool have_position_error   = sqrt(pow(mpc_x(0, 0) - des_x, 2) + pow(mpc_x(4, 0) - des_y, 2) + pow(mpc_x(8, 0) - des_z, 2)) > _diag_pos_tracking_thr_;
+    1137        9231 :   bool have_heading_error    = fabs(radians::diff(mpc_x_heading(0), des_heading)) > _diag_heading_tracking_thr_;
+    1138        9231 :   bool have_nonzero_velocity = fabs(mpc_x(1, 0)) > 0.1 || fabs(mpc_x(5, 0)) > 0.1 || fabs(mpc_x(9, 0)) > 0.1 || fabs(mpc_x_heading(1, 0)) > 0.1;
+    1139             : 
+    1140        9231 :   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
+    1141             : 
+    1142        9231 :   if (!is_active_)
+    1143             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+    1144        9231 :   else if (tracker_status.tracking_trajectory)
+    1145        9231 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_TRAJECTORY;
+    1146             :   else if (tracker_status.have_goal)
+    1147        9231 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+    1148             :   else
+    1149        3378 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_HOVER;
+    1150             : 
+    1151        3378 : 
+    1152             :   int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    1153        1689 : 
+    1154        1689 :   tracker_status.trajectory_length = trajectory_size;
+    1155             :   tracker_status.trajectory_idx    = trajectory_tracking_idx;
+    1156        1689 : 
+    1157        1689 :   if (trajectory_tracking_in_progress_) {
+    1158        1689 : 
+    1159        1689 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1160             : 
+    1161             :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    1162             : 
+    1163        3378 :     tracker_status.trajectory_reference.header.stamp    = ros::Time::now();
+    1164        1689 :     tracker_status.trajectory_reference.header.frame_id = uav_state.header.frame_id;
+    1165        1689 : 
+    1166             :     tracker_status.trajectory_reference.reference.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1167        1689 :     tracker_status.trajectory_reference.reference.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1168        1689 :     tracker_status.trajectory_reference.reference.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1169        1689 :     tracker_status.trajectory_reference.reference.heading    = (*des_heading_whole_trajectory_)(trajectory_tracking_idx);
+    1170             : 
+    1171        1689 :     // | ---------- publish the current trajectory point ---------- |
+    1172             : 
+    1173        1689 :     geometry_msgs::PoseStamped debug_trajectory_point;
+    1174             :     debug_trajectory_point.header.stamp    = ros::Time::now();
+    1175             :     debug_trajectory_point.header.frame_id = uav_state_.header.frame_id;
+    1176       18462 : 
+    1177             :     debug_trajectory_point.pose.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1178             :     debug_trajectory_point.pose.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1179             :     debug_trajectory_point.pose.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1180             : 
+    1181             :     debug_trajectory_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(trajectory_tracking_idx));
+    1182             : 
+    1183        1975 :     ph_current_trajectory_point_.publish(debug_trajectory_point);
+    1184             :   }
+    1185        3950 : 
+    1186             :   return tracker_status;
+    1187        1975 : }
+    1188             : 
+    1189        1699 : //}
+    1190        1699 : 
+    1191             : /* //{ enableCallbacks() */
+    1192             : 
+    1193             : const std_srvs::SetBoolResponse::ConstPtr MpcTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+    1194         276 : 
+    1195             :   std::stringstream ss;
+    1196             : 
+    1197        3950 :   if (cmd->data != callbacks_enabled_) {
+    1198        1975 : 
+    1199        1975 :     callbacks_enabled_ = cmd->data;
+    1200             :     ss << "callbacks %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1201        3950 : 
+    1202             :   } else {
+    1203             : 
+    1204             :     ss << "callbacks were already %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1205             :   }
+    1206             : 
+    1207             :   std_srvs::SetBoolResponse res;
+    1208           5 :   res.message = ss.str();
+    1209             :   res.success = true;
+    1210           5 : 
+    1211           5 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+    1212             : }
+    1213          10 : 
+    1214          10 : //}
+    1215             : 
+    1216           5 : /* switchOdometrySource() //{ */
+    1217             : 
+    1218             : const std_srvs::TriggerResponse::ConstPtr MpcTracker::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+    1219             : 
+    1220             :   odometry_reset_in_progress_ = true;
+    1221             :   mpc_result_invalid_         = true;
+    1222             : 
+    1223             :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1224           5 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1225           5 : 
+    1226             :   ROS_INFO(
+    1227           5 :       "[MpcTracker]: start of odometry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1228             :       "%.2f], "
+    1229           3 :       "new odom [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: %.2f]",
+    1230           3 :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0), new_uav_state.pose.position.x, new_uav_state.pose.position.y,
+    1231           3 :       new_uav_state.pose.position.z, new_uav_state.velocity.linear.x, new_uav_state.velocity.linear.y, new_uav_state.velocity.linear.z,
+    1232             :       new_uav_state.acceleration.linear.x, new_uav_state.acceleration.linear.y, new_uav_state.acceleration.linear.z);
+    1233           3 : 
+    1234           3 :   timer_mpc_iteration_.stop();
+    1235           3 :   ROS_INFO("[MpcTracker]: mpc timer stopped");
+    1236             : 
+    1237             :   while (mpc_timer_running_) {
+    1238             : 
+    1239             :     ROS_DEBUG("[MpcTracker]: the mpc is in the middle of an iteration, waiting for it to finish");
+    1240             :     ros::Duration wait(0.001);
+    1241           5 :     wait.sleep();
+    1242           5 : 
+    1243           5 :     if (!mpc_timer_running_) {
+    1244             :       ROS_DEBUG("[MpcTracker]: mpc timer finished");
+    1245             :       break;
+    1246           5 :     }
+    1247             :   }
+    1248           0 : 
+    1249           0 :   // | --------- recalculate the goal to new coordinates -------- |
+    1250           0 : 
+    1251             :   double old_heading  = 0;
+    1252             :   double new_heading  = 0;
+    1253             :   bool   got_headings = true;
+    1254           5 : 
+    1255             :   try {
+    1256           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1257           0 :   }
+    1258           0 :   catch (...) {
+    1259             :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+    1260             :     got_headings = false;
+    1261          10 :   }
+    1262             : 
+    1263           5 :   try {
+    1264           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+    1265           0 :   }
+    1266             :   catch (...) {
+    1267           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+    1268             :     got_headings = false;
+    1269             :   }
+    1270             : 
+    1271           5 :   std_srvs::TriggerResponse res;
+    1272           5 : 
+    1273           5 :   if (!got_headings) {
+    1274           5 :     res.message = "could not calculate the heading difference";
+    1275             :     res.success = false;
+    1276           5 : 
+    1277             :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1278             :   }
+    1279           5 : 
+    1280             :   // calculate the difference of position
+    1281           5 :   double dx       = new_uav_state.pose.position.x - uav_state_.pose.position.x;
+    1282             :   double dy       = new_uav_state.pose.position.y - uav_state_.pose.position.y;
+    1283           0 :   double dz       = new_uav_state.pose.position.z - uav_state_.pose.position.z;
+    1284             :   double dheading = new_heading - old_heading;
+    1285           0 : 
+    1286           0 :   ROS_INFO("[MpcTracker]: dx %f dy %f dz %f dheading %f", dx, dy, dz, dheading);
+    1287             : 
+    1288           0 :   {
+    1289           0 :     std::scoped_lock lock(mutex_mpc_x_, mutex_des_trajectory_, mutex_des_whole_trajectory_, mutex_uav_state_);
+    1290           0 : 
+    1291           0 :     if (trajectory_set_) {
+    1292             : 
+    1293             :       for (int i = 0; i < trajectory_size_ + MPC_HORIZON_LENGTH; i++) {
+    1294             : 
+    1295         205 :         Eigen::Vector2d temp_vec((*des_x_whole_trajectory_)(i)-uav_state_.pose.position.x, (*des_y_whole_trajectory_)(i)-uav_state_.pose.position.y);
+    1296             :         temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1297         200 : 
+    1298         200 :         (*des_x_whole_trajectory_)(i) = new_uav_state.pose.position.x + temp_vec(0);
+    1299             :         (*des_y_whole_trajectory_)(i) = new_uav_state.pose.position.y + temp_vec(1);
+    1300         200 :         (*des_z_whole_trajectory_)(i) += dz;
+    1301         200 :         (*des_heading_whole_trajectory_)(i) += dheading;
+    1302         200 :       }
+    1303         200 :     }
+    1304             : 
+    1305             :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1306             : 
+    1307             :       Eigen::Vector2d temp_vec(des_x_trajectory_(i) - uav_state_.pose.position.x, des_y_trajectory_(i) - uav_state_.pose.position.y);
+    1308           5 :       temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1309           5 : 
+    1310           5 :       des_x_trajectory_(i, 0) = new_uav_state.pose.position.x + temp_vec(0);
+    1311           5 :       des_y_trajectory_(i, 0) = new_uav_state.pose.position.y + temp_vec(1);
+    1312           5 :       des_z_trajectory_(i, 0) += dz;
+    1313             :       des_heading_trajectory_(i, 0) += dheading;
+    1314             :     }
+    1315             : 
+    1316             :     // update the position
+    1317           5 :     {
+    1318           5 :       Eigen::Vector2d temp_vec(mpc_x_(0, 0) - uav_state_.pose.position.x, mpc_x_(4, 0) - uav_state_.pose.position.y);
+    1319             :       temp_vec     = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1320             :       mpc_x_(0, 0) = new_uav_state.pose.position.x + temp_vec(0);
+    1321             :       mpc_x_(4, 0) = new_uav_state.pose.position.y + temp_vec(1);
+    1322             :       mpc_x_(8, 0) += dz;
+    1323             :     }
+    1324           5 : 
+    1325           5 :     // update the velocity
+    1326           5 :     {
+    1327             :       mpc_x_(1, 0) = new_uav_state.velocity.linear.x;
+    1328             :       mpc_x_(5, 0) = new_uav_state.velocity.linear.y;
+    1329             :       // we leave the z velocity as it was in the original frame
+    1330           5 :     }
+    1331           5 : 
+    1332             :     // update the acceleration
+    1333             :     {
+    1334           5 :       mpc_x_(2, 0)  = 0;
+    1335             :       mpc_x_(6, 0)  = 0;
+    1336             :       mpc_x_(10, 0) = 0;
+    1337             :     }
+    1338             : 
+    1339           5 :     // update the heading and its derivative
+    1340             :     mpc_x_heading_(0, 0) += dheading;
+    1341           5 :     mpc_x_heading_(1, 0) = new_uav_state.velocity.angular.x;
+    1342           5 :   }
+    1343             : 
+    1344             :   ROS_INFO(
+    1345           5 :       "[MpcTracker]: start of odometry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1346             :       "%.2f]",
+    1347           5 :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0));
+    1348             : 
+    1349             :   ROS_INFO("[MpcTracker]: starting the MPC timer");
+    1350             : 
+    1351             :   if (!mpc_synchronous_) {
+    1352             :     timer_mpc_iteration_.start();
+    1353             :   }
+    1354           1 : 
+    1355             :   odometry_reset_in_progress_ = false;
+    1356           1 : 
+    1357             :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1358           2 : }
+    1359           1 : 
+    1360             : //}
+    1361           2 : 
+    1362           1 : /* //{ hover() */
+    1363           1 : 
+    1364             : const std_srvs::TriggerResponse::ConstPtr MpcTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1365           2 : 
+    1366             :   toggleHover(true);
+    1367             : 
+    1368             :   std::stringstream ss;
+    1369             :   ss << "initiating hover";
+    1370             : 
+    1371             :   std_srvs::TriggerResponse res;
+    1372           2 :   res.success = true;
+    1373           4 :   res.message = ss.str();
+    1374             : 
+    1375           4 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1376             : }
+    1377           4 : 
+    1378           2 : //}
+    1379           2 : 
+    1380             : /* //{ startTrajectoryTracking() */
+    1381           4 : 
+    1382             : const std_srvs::TriggerResponse::ConstPtr MpcTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1383             :   std::stringstream ss;
+    1384             : 
+    1385             :   auto [success, message] = startTrajectoryTrackingImpl();
+    1386             : 
+    1387             :   std_srvs::TriggerResponse res;
+    1388           1 :   res.success = success;
+    1389             :   res.message = message;
+    1390           2 : 
+    1391             :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1392           2 : }
+    1393           1 : 
+    1394           1 : //}
+    1395             : 
+    1396           2 : /* //{ stopTrajectoryTracking() */
+    1397             : 
+    1398             : const std_srvs::TriggerResponse::ConstPtr MpcTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1399             : 
+    1400             :   auto [success, message] = stopTrajectoryTrackingImpl();
+    1401             : 
+    1402             :   std_srvs::TriggerResponse res;
+    1403           1 :   res.success = success;
+    1404             :   res.message = message;
+    1405           2 : 
+    1406             :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1407           2 : }
+    1408           1 : 
+    1409           1 : //}
+    1410             : 
+    1411           2 : /* //{ resumeTrajectoryTracking() */
+    1412             : 
+    1413             : const std_srvs::TriggerResponse::ConstPtr MpcTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1414             : 
+    1415             :   auto [success, message] = resumeTrajectoryTrackingImpl();
+    1416             : 
+    1417             :   std_srvs::TriggerResponse res;
+    1418           2 :   res.success = success;
+    1419             :   res.message = message;
+    1420           4 : 
+    1421             :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1422           4 : }
+    1423           2 : 
+    1424           2 : //}
+    1425             : 
+    1426           4 : /* //{ gotoTrajectoryStart() */
+    1427             : 
+    1428             : const std_srvs::TriggerResponse::ConstPtr MpcTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1429             : 
+    1430             :   auto [success, message] = gotoTrajectoryStartImpl();
+    1431             : 
+    1432             :   std_srvs::TriggerResponse res;
+    1433         375 :   res.success = success;
+    1434             :   res.message = message;
+    1435         375 : 
+    1436           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1437             : }
+    1438             : 
+    1439         375 : //}
+    1440             : 
+    1441             : /* //{ setConstraints() */
+    1442             : 
+    1443             : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+    1444         750 : 
+    1445             :   if (!is_initialized_) {
+    1446             :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+    1447         375 :   }
+    1448             : 
+    1449         107 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+    1450             : 
+    1451             :   // directly updated the speeds in the constraints
+    1452             :   // the reset needs to wait for manageConstraints()
+    1453         268 :   {
+    1454         268 :     std::scoped_lock lock(mutex_constraints_filtered_);
+    1455         268 : 
+    1456         268 :     // important! this needs to be done to initialize the full struct
+    1457             :     if (!got_constraints_) {
+    1458             : 
+    1459             :       constraints_filtered_ = constraints->constraints;
+    1460         375 : 
+    1461             :     } else {
+    1462         375 : 
+    1463             :       constraints_filtered_.horizontal_speed          = constraints->constraints.horizontal_speed;
+    1464         375 :       constraints_filtered_.vertical_ascending_speed  = constraints->constraints.vertical_ascending_speed;
+    1465             :       constraints_filtered_.vertical_descending_speed = constraints->constraints.vertical_descending_speed;
+    1466         750 :       constraints_filtered_.heading_speed             = constraints->constraints.heading_speed;
+    1467         375 :     }
+    1468         375 :   }
+    1469             : 
+    1470         375 :   got_constraints_ = true;
+    1471             : 
+    1472             :   all_constraints_set_ = false;
+    1473             : 
+    1474             :   ROS_INFO("[MpcTracker]: updating constraints");
+    1475             : 
+    1476             :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+    1477         260 :   res.success = true;
+    1478             :   res.message = "constraints updated";
+    1479         260 : 
+    1480             :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+    1481         260 : }
+    1482             : 
+    1483         520 : //}
+    1484         260 : 
+    1485         260 : /* //{ setReference() */
+    1486             : 
+    1487         520 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MpcTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+    1488             : 
+    1489             :   toggleHover(false);
+    1490             : 
+    1491             :   setGoal(cmd->reference.position.x, cmd->reference.position.y, cmd->reference.position.z, cmd->reference.heading, true);
+    1492             : 
+    1493             :   mrs_msgs::ReferenceSrvResponse res;
+    1494         737 :   res.success = true;
+    1495             :   res.message = "reference set";
+    1496         737 : 
+    1497           0 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+    1498             : }
+    1499             : 
+    1500             : //}
+    1501         737 : 
+    1502             : /* //{ setVelocityReference() */
+    1503         737 : 
+    1504             : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MpcTracker::setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+    1505         737 : 
+    1506             :   if (!is_initialized_) {
+    1507             :     return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+    1508         737 :   }
+    1509             : 
+    1510           4 :   {
+    1511             :     std::scoped_lock lock(mutex_velocity_reference_);
+    1512           4 : 
+    1513           4 :     velocity_reference_time_ = ros::Time::now();
+    1514             : 
+    1515           4 :     velocity_reference_ = cmd->reference;
+    1516             :   }
+    1517             : 
+    1518        1474 :   if (!velocity_tracking_active_) {
+    1519         737 : 
+    1520         737 :     ROS_INFO("[MpcTracker]: starting velocity tracking timer");
+    1521             : 
+    1522         737 :     timer_velocity_tracking_.stop();
+    1523             :     timer_velocity_tracking_.start();
+    1524             : 
+    1525             :     velocity_tracking_active_ = true;
+    1526             :   }
+    1527             : 
+    1528             :   mrs_msgs::VelocityReferenceSrvResponse response;
+    1529           6 :   response.success = true;
+    1530             :   response.message = "reference set";
+    1531             : 
+    1532          12 :   return mrs_msgs::VelocityReferenceSrvResponse::ConstPtr(new mrs_msgs::VelocityReferenceSrvResponse(response));
+    1533             : }
+    1534          18 : 
+    1535             : //}
+    1536          12 : 
+    1537           6 : /* //{ setTrajectoryReference() */
+    1538           6 : 
+    1539           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MpcTracker::setTrajectoryReference([
+    1540             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+    1541          12 : 
+    1542             :   std::stringstream ss;
+    1543             : 
+    1544             :   auto [success, message, modified] = loadTrajectory(cmd->trajectory);
+    1545             : 
+    1546             :   mrs_msgs::TrajectoryReferenceSrvResponse response;
+    1547             :   response.success  = success;
+    1548             :   response.message  = message;
+    1549             :   response.modified = modified;
+    1550         338 : 
+    1551             :   return mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr(new mrs_msgs::TrajectoryReferenceSrvResponse(response));
+    1552         338 : }
+    1553           0 : 
+    1554             : //}
+    1555             : 
+    1556         676 : // | ------------------------ callbacks ----------------------- |
+    1557             : 
+    1558         676 : /* //{ callbackOtherMavTrajectory() */
+    1559             : 
+    1560         338 : void MpcTracker::callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg) {
+    1561             : 
+    1562         338 :   if (!is_initialized_) {
+    1563             :     return;
+    1564             :   }
+    1565         338 : 
+    1566             :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavTrajectory");
+    1567             :   mrs_lib::ScopeTimer timer =
+    1568         676 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1569             : 
+    1570         338 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1571             : 
+    1572           0 :   mrs_msgs::FutureTrajectory trajectory = *msg;
+    1573           0 : 
+    1574           0 :   // the times might not be synchronized, so just remember the time of receiving it
+    1575             :   trajectory.stamp = ros::Time::now();
+    1576           0 : 
+    1577             :   // transform it from the utm origin to the currently used frame
+    1578             :   auto res = common_handlers_->transformer->getTransform("utm_origin", uav_state.header.frame_id, ros::Time::now());
+    1579         338 : 
+    1580             :   if (!res) {
+    1581       13858 : 
+    1582             :     std::string message = "[MpcTracker]: can not transform other drone trajectory to the current frame";
+    1583       13520 :     ROS_WARN_STREAM_ONCE(message);
+    1584             :     ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1585       13520 : 
+    1586       13520 :     return;
+    1587       13520 :   }
+    1588             : 
+    1589       13520 :   geometry_msgs::TransformStamped tf = res.value();
+    1590             : 
+    1591       13520 :   for (int i = 0; i < int(trajectory.points.size()); i++) {
+    1592             : 
+    1593       13520 :     geometry_msgs::PoseStamped original_pose;
+    1594       13520 : 
+    1595       13520 :     original_pose.pose.position.x = trajectory.points.at(i).x;
+    1596       13520 :     original_pose.pose.position.y = trajectory.points.at(i).y;
+    1597             :     original_pose.pose.position.z = trajectory.points.at(i).z;
+    1598             : 
+    1599           0 :     original_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1600           0 : 
+    1601           0 :     auto res = common_handlers_->transformer->transform(original_pose, tf);
+    1602             : 
+    1603           0 :     if (res) {
+    1604             :       trajectory.points.at(i).x = res.value().pose.position.x;
+    1605             :       trajectory.points.at(i).y = res.value().pose.position.y;
+    1606             :       trajectory.points.at(i).z = res.value().pose.position.z;
+    1607             :     } else {
+    1608         676 : 
+    1609             :       std::string message = "[MpcTracker]: could not transform point of other uav future trajectory!";
+    1610             :       ROS_WARN_STREAM_ONCE(message);
+    1611         338 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1612             : 
+    1613             :       return;
+    1614             :     }
+    1615             :   }
+    1616             : 
+    1617             :   {
+    1618             :     std::scoped_lock lock(mutex_other_uav_avoidance_trajectories_);
+    1619        2151 : 
+    1620             :     // update the diagnostics
+    1621        6453 :     other_uav_avoidance_trajectories_[trajectory.uav_name] = trajectory;
+    1622             :   }
+    1623        6453 : }
+    1624             : 
+    1625        4302 : //}
+    1626             : 
+    1627             : /* //{ callbackOtherMavDiagnostics() */
+    1628             : 
+    1629        2151 : void MpcTracker::callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg) {
+    1630             : 
+    1631             :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavDiagnostics");
+    1632        4302 :   mrs_lib::ScopeTimer timer =
+    1633             :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1634        2151 : 
+    1635             :   mrs_msgs::MpcTrackerDiagnostics diagnostics = *msg;
+    1636        2151 : 
+    1637             :   // fill in the current time
+    1638             :   // the other uav's time might not be synchronized with ours
+    1639             :   diagnostics.header.stamp = ros::Time::now();
+    1640             : 
+    1641             :   {
+    1642           0 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    1643             : 
+    1644           0 :     other_uav_diagnostics_[diagnostics.uav_name] = diagnostics;
+    1645             :   }
+    1646           0 : }
+    1647             : 
+    1648           0 : //}
+    1649           0 : 
+    1650             : /* //{ callbackToggleCollisionAvoidance() */
+    1651           0 : 
+    1652             : bool MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1653             : 
+    1654             :   collision_avoidance_enabled_ = req.data;
+    1655             : 
+    1656             :   ROS_INFO("[MpcTracker]: Collision avoidance was switched %s", collision_avoidance_enabled_ ? "TRUE" : "FALSE");
+    1657             : 
+    1658           0 :   res.message = "Collision avoidance set.";
+    1659             :   res.success = true;
+    1660           0 : 
+    1661             :   return true;
+    1662           0 : }
+    1663           0 : 
+    1664           0 : //}
+    1665             : 
+    1666             : /* callbackWiggle() //{ */
+    1667             : 
+    1668           0 : bool MpcTracker::callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1669             : 
+    1670           0 :   if (!is_initialized_) {
+    1671             : 
+    1672           0 :     res.success = false;
+    1673             :     res.message = "tracker not active";
+    1674             :     return true;
+    1675           0 :   }
+    1676           0 : 
+    1677             :   {
+    1678           0 :     std::scoped_lock lock(mutex_drs_params_);
+    1679             : 
+    1680             :     drs_params_.wiggle_enabled = req.data;
+    1681             : 
+    1682             :     reconfigure_server_->updateConfig(drs_params_);
+    1683             :   }
+    1684             : 
+    1685         107 :   res.success = true;
+    1686             :   res.message = "wiggle updated";
+    1687         214 : 
+    1688             :   return true;
+    1689         107 : }
+    1690             : 
+    1691         107 : //}
+    1692         107 : 
+    1693             : /* //{ dynamicReconfigureCallback() */
+    1694             : 
+    1695             : void MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, [[maybe_unused]] uint32_t level) {
+    1696             : 
+    1697             :   std::scoped_lock lock(mutex_drs_params_);
+    1698             : 
+    1699             :   drs_params_ = config;
+    1700             : 
+    1701             :   ROS_INFO("[MpcTracker]: DRS updated");
+    1702             : }
+    1703             : 
+    1704      186640 : //}
+    1705             : 
+    1706      186640 : // --------------------------------------------------------------
+    1707             : // |                          routines                          |
+    1708       10756 : // --------------------------------------------------------------
+    1709             : 
+    1710             : // | --------------- mutual collision avoidance --------------- |
+    1711             : 
+    1712      175884 : /* //{ checkCollision() */
+    1713             : 
+    1714             : bool MpcTracker::checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1715             : 
+    1716             :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ && fabs(az - bz) < _avoidance_z_threshold_) {
+    1717             : 
+    1718             :     return true;
+    1719             : 
+    1720      186640 :   } else {
+    1721             : 
+    1722      186640 :     return false;
+    1723             :   }
+    1724       67469 : }
+    1725             : 
+    1726             : //}
+    1727             : 
+    1728      119171 : /* //{ checkCollisionInflated() */
+    1729             : 
+    1730             : bool MpcTracker::checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1731             : 
+    1732             :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ + 1.0 && fabs(az - bz) < _avoidance_z_threshold_ + 1.0) {
+    1733             : 
+    1734             :     return true;
+    1735             : 
+    1736             :   } else {
+    1737       85596 : 
+    1738             :     return false;
+    1739       85596 :   }
+    1740             : }
+    1741       85596 : 
+    1742       85596 : //}
+    1743             : 
+    1744       85596 : /* //{ checkTrajectoryForCollisions() */
+    1745             : 
+    1746       90262 : // Check for potential collisions and return the needed altitude offset to avoid other drones
+    1747             : double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
+    1748             : 
+    1749        4666 :   std::scoped_lock lock(mutex_predicted_trajectory_, mutex_des_trajectory_, mutex_other_uav_avoidance_trajectories_);
+    1750             : 
+    1751      191306 :   first_collision_index = INT_MAX;
+    1752             :   avoiding_collision_   = false;
+    1753             : 
+    1754      186640 :   std::map<std::string, mrs_msgs::FutureTrajectory>::iterator u = other_uav_avoidance_trajectories_.begin();
+    1755      186640 : 
+    1756             :   while (u != other_uav_avoidance_trajectories_.end()) {
+    1757             : 
+    1758       10756 :     // is the other's trajectory fresh enought?
+    1759             :     if ((ros::Time::now() - u->second.stamp).toSec() < _collision_trajectory_timeout_) {
+    1760             : 
+    1761       10756 :       for (int v = 0; v < MPC_HORIZON_LENGTH; v++) {
+    1762             : 
+    1763             :         // check all points of the trajectory for possible collisions
+    1764       10756 :         if (checkCollision(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1765             :                            predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points.at(v).x, u->second.points.at(v).y, u->second.points.at(v).z)) {
+    1766             : 
+    1767        6561 :           // collision is detected
+    1768        6561 :           int other_uav_priority = INT_MAX;
+    1769             :           // get the priority of the other uav
+    1770        6561 :           /* sscanf(u->first.c_str(), "uav%d", &other_uav_priority); */
+    1771         214 :           other_uav_priority = u->second.priority;
+    1772             : 
+    1773             :           // check if we should be avoiding (out priority is higher, or the other uav has collision avoidance turned off)
+    1774        6561 :           if ((u->second.collision_avoidance == false) || (other_uav_priority < avoidance_this_uav_priority_)) {
+    1775             : 
+    1776             :             // we should be avoiding
+    1777             :             avoiding_collision_      = true;
+    1778        4195 :             double tmp_safe_altitude = u->second.points.at(v).z + _avoidance_z_correction_;
+    1779             : 
+    1780             :             if (tmp_safe_altitude > collision_free_altitude_ && v <= _avoidance_collision_start_climbing_) {
+    1781             :               collision_free_altitude_ = tmp_safe_altitude;
+    1782      186640 :             }
+    1783      186640 : 
+    1784      186640 :             ROS_ERROR_STREAM_THROTTLE(1, "[MpcTracker]: avoiding collision with uav" << other_uav_priority);
+    1785             : 
+    1786             :           } else {
+    1787       67469 :             // the other uav should avoid us
+    1788        2578 :             ROS_WARN_STREAM_THROTTLE(1, "[MpcTracker]: detected collision with uav" << other_uav_priority << ", not avoiding (my priority is higher)");
+    1789             :           }
+    1790             :         }
+    1791             : 
+    1792             :         if (checkCollisionInflated(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1793             :                                    predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points.at(v).x, u->second.points.at(v).y,
+    1794        4666 :                                    u->second.points.at(v).z)) {
+    1795             : 
+    1796             :           // collision is detected
+    1797       85596 :           if (first_collision_index > v) {
+    1798             :             first_collision_index = v;
+    1799       84907 :           }
+    1800             :         }
+    1801             :       }
+    1802       84907 :     }
+    1803             : 
+    1804       84907 :     u++;
+    1805             :   }
+    1806       84907 : 
+    1807             :   if (!avoiding_collision_) {
+    1808           0 : 
+    1809             :     auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1810             : 
+    1811             :     // we are not avoiding any collisions, so we slowly reduce the collision avoidance offset to return to normal flight
+    1812      171192 :     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
+    1813             : 
+    1814             :     double safety_area_min_z = std::numeric_limits<double>::lowest();
+    1815             : 
+    1816             :     if (collision_free_altitude_ < safety_area_min_z) {
+    1817             : 
+    1818             :       collision_free_altitude_ = safety_area_min_z;
+    1819             :     }
+    1820             :   }
+    1821       86608 : 
+    1822             :   return collision_free_altitude_;
+    1823             : }
+    1824       86608 : 
+    1825             : //}
+    1826      173216 : 
+    1827       86608 : // | ------------------ trajectory filtering ------------------ |
+    1828             : 
+    1829      173216 : /* //{ filterReferenceXY() */
+    1830      173216 : 
+    1831             : std::tuple<MatrixXd, MatrixXd> MpcTracker::filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x,
+    1832             :                                                              double max_speed_y) {
+    1833             : 
+    1834             :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1835             : 
+    1836             :   auto mpc_x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1837     3550932 :   auto trajectory_dt = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_dt_);
+    1838             : 
+    1839     3464320 :   MatrixXd filtered_x_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1840       86608 :   MatrixXd filtered_y_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1841       86608 : 
+    1842       86608 :   double difference_x;
+    1843       86608 :   double difference_y;
+    1844             :   double max_sample_x;
+    1845     3377708 :   double max_sample_y;
+    1846     3377708 : 
+    1847     3377708 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1848     3377708 : 
+    1849             :     if (i == 0) {
+    1850             :       max_sample_x = max_speed_x * dt1;
+    1851     3464320 :       max_sample_y = max_speed_y * dt1;
+    1852             :       difference_x = des_x_trajectory(i, 0) - mpc_x(0, 0);
+    1853     2840800 :       difference_y = des_y_trajectory(i, 0) - mpc_x(4, 0);
+    1854     2840800 :     } else {
+    1855     2840800 :       max_sample_x = max_speed_x * _dt2_;
+    1856             :       max_sample_y = max_speed_y * _dt2_;
+    1857     2840800 :       difference_x = des_x_trajectory(i, 0) - filtered_x_trajectory(i - 1, 0);
+    1858      307611 :       difference_y = des_y_trajectory(i, 0) - filtered_y_trajectory(i - 1, 0);
+    1859             :     }
+    1860     2840800 : 
+    1861     2839967 :     if (!trajectory_tracking_in_progress_) {
+    1862             : 
+    1863             :       double direction_angle  = atan2(difference_y, difference_x);
+    1864             :       double max_dir_sample_x = abs(max_sample_x * cos(direction_angle));
+    1865     2840800 :       double max_dir_sample_y = abs(max_sample_y * sin(direction_angle));
+    1866      174886 : 
+    1867     2665917 :       if (max_sample_x > max_dir_sample_x) {
+    1868      194491 :         max_sample_x = max_dir_sample_x;
+    1869             :       }
+    1870     2840800 :       if (max_sample_y > max_dir_sample_y) {
+    1871       89280 :         max_sample_y = max_dir_sample_y;
+    1872     2751523 :       }
+    1873      224339 : 
+    1874             :       // saturate the difference
+    1875             :       if (difference_x > max_sample_x)
+    1876     3464320 :         difference_x = max_sample_x;
+    1877       86608 :       else if (difference_x < -max_sample_x)
+    1878       86608 :         difference_x = -max_sample_x;
+    1879             : 
+    1880     3377708 :       if (difference_y > max_sample_y)
+    1881     3377708 :         difference_y = max_sample_y;
+    1882             :       else if (difference_y < -max_sample_y)
+    1883             :         difference_y = -max_sample_y;
+    1884             :     }
+    1885             : 
+    1886             :     if (i == 0) {
+    1887       86608 :       filtered_x_trajectory(i, 0) = mpc_x(0, 0) + difference_x;
+    1888       86608 :       filtered_y_trajectory(i, 0) = mpc_x(4, 0) + difference_y;
+    1889             :     } else {
+    1890       86608 :       filtered_x_trajectory(i, 0) = filtered_x_trajectory(i - 1, 0) + difference_x;
+    1891             :       filtered_y_trajectory(i, 0) = filtered_y_trajectory(i - 1, 0) + difference_y;
+    1892           0 :     }
+    1893           0 :   }
+    1894           0 : 
+    1895             :   // | ----------------------- add wiggle ----------------------- |
+    1896             : 
+    1897           0 :   auto [wiggle_enabled, wiggle_amplitude, wiggle_frequency_] =
+    1898             :       mrs_lib::get_mutexed(mutex_drs_params_, drs_params_.wiggle_enabled, drs_params_.wiggle_amplitude, drs_params_.wiggle_frequency);
+    1899           0 : 
+    1900           0 :   if (wiggle_enabled) {
+    1901             : 
+    1902             :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1903             :       filtered_x_trajectory(i, 0) += wiggle_amplitude * cos(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1904      173216 :       filtered_y_trajectory(i, 0) += wiggle_amplitude * sin(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1905             :     }
+    1906             : 
+    1907             :     wiggle_phase_ += wiggle_frequency_ * dt1 * 2 * M_PI;
+    1908             : 
+    1909             :     if (wiggle_phase_ > M_PI) {
+    1910             :       wiggle_phase_ -= 2 * M_PI;
+    1911       86608 :     }
+    1912             :   }
+    1913      173216 : 
+    1914             :   return std::make_tuple(filtered_x_trajectory, filtered_y_trajectory);
+    1915       86608 : }
+    1916             : 
+    1917             : //}
+    1918             : 
+    1919             : /* //{ filterReferenceZ() */
+    1920       86608 : 
+    1921             : MatrixXd MpcTracker::filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed) {
+    1922       86608 : 
+    1923             :   auto mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1924     3550932 : 
+    1925             :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1926     3464320 : 
+    1927             :   double difference_z;
+    1928       86608 :   double max_sample_z;
+    1929             : 
+    1930       86608 :   MatrixXd filtered_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1931       30126 : 
+    1932             :   double current_z = mpc_x(8, 0);
+    1933       56482 : 
+    1934             :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1935             : 
+    1936             :     if (i == 0) {
+    1937             : 
+    1938     3377708 :       difference_z = des_z_trajectory(i, 0) - current_z;
+    1939             : 
+    1940     3377708 :       if (difference_z > 0) {
+    1941      206116 :         max_sample_z = max_ascending_speed * dt1;
+    1942             :       } else {
+    1943     3171595 :         max_sample_z = max_descending_speed * dt1;
+    1944             :       }
+    1945             : 
+    1946             :     } else {
+    1947     3464320 : 
+    1948             :       difference_z = des_z_trajectory(i, 0) - filtered_trajectory(i - 1, 0);
+    1949             : 
+    1950     2840960 :       if (difference_z > 0) {
+    1951       86307 :         max_sample_z = max_ascending_speed * _dt2_;
+    1952     2754657 :       } else {
+    1953       22372 :         max_sample_z = max_descending_speed * _dt2_;
+    1954             :       }
+    1955             :     }
+    1956     3464320 : 
+    1957       86608 :     if (!trajectory_tracking_in_progress_) {
+    1958             : 
+    1959     3377708 :       // saturate the difference
+    1960             :       if (difference_z > max_sample_z)
+    1961             :         difference_z = max_sample_z;
+    1962             :       else if (difference_z < -max_sample_z)
+    1963      173216 :         difference_z = -max_sample_z;
+    1964             :     }
+    1965             : 
+    1966             :     if (i == 0) {
+    1967             :       filtered_trajectory(i, 0) = current_z + difference_z;
+    1968             :     } else {
+    1969             :       filtered_trajectory(i, 0) = filtered_trajectory(i - 1, 0) + difference_z;
+    1970       86608 :     }
+    1971             :   }
+    1972       86608 : 
+    1973       86433 :   return filtered_trajectory;
+    1974             : }
+    1975             : 
+    1976       86608 : //}
+    1977       86433 : 
+    1978             : /* //{ manageConstraints() */
+    1979             : 
+    1980         175 : void MpcTracker::manageConstraints() {
+    1981         350 : 
+    1982             :   if (!got_constraints_) {
+    1983         350 :     return;
+    1984         175 :   }
+    1985         175 : 
+    1986         175 :   if (all_constraints_set_) {
+    1987         175 :     return;
+    1988         175 :   }
+    1989         525 : 
+    1990         175 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1991             :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1992         175 : 
+    1993             :   bool can_change = (fabs(mpc_x(1, 0)) < constraints.horizontal_speed) && (fabs(mpc_x(2, 0)) < constraints.horizontal_acceleration) &&
+    1994             :                     (fabs(mpc_x(3, 0)) < constraints.horizontal_jerk) && (fabs(mpc_x(5, 0)) < constraints.horizontal_speed) &&
+    1995         175 :                     (fabs(mpc_x(6, 0)) < constraints.horizontal_acceleration) && (fabs(mpc_x(7, 0)) < constraints.horizontal_jerk) &&
+    1996             :                     (mpc_x(9, 0) < constraints.vertical_ascending_speed) && (mpc_x(9, 0) > -constraints.vertical_descending_speed) &&
+    1997         175 :                     (mpc_x(10, 0) < constraints.vertical_ascending_acceleration) && (mpc_x(10, 0) > -constraints.vertical_descending_acceleration) &&
+    1998         175 :                     (mpc_x(11, 0) < constraints.vertical_ascending_jerk) && (mpc_x(11, 0) > -constraints.vertical_descending_jerk) &&
+    1999         175 :                     (fabs(mpc_x_heading(1, 0)) < constraints.heading_speed) && (fabs(mpc_x_heading(2, 0)) < constraints.heading_acceleration) &&
+    2000             :                     (fabs(mpc_x_heading(3, 0)) < constraints.heading_jerk);
+    2001         175 : 
+    2002         175 :   if (can_change) {
+    2003         175 : 
+    2004             :     {
+    2005         175 :       std::scoped_lock lock(mutex_constraints_filtered_);
+    2006         175 : 
+    2007         175 :       constraints_filtered_.horizontal_acceleration = constraints.horizontal_acceleration;
+    2008             :       constraints_filtered_.horizontal_jerk         = constraints.horizontal_jerk;
+    2009         175 :       constraints_filtered_.horizontal_snap         = constraints.horizontal_snap;
+    2010         175 : 
+    2011         175 :       constraints_filtered_.vertical_ascending_acceleration = constraints.vertical_ascending_acceleration;
+    2012             :       constraints_filtered_.vertical_ascending_jerk         = constraints.vertical_ascending_jerk;
+    2013             :       constraints_filtered_.vertical_ascending_snap         = constraints.vertical_ascending_snap;
+    2014         175 : 
+    2015         175 :       constraints_filtered_.vertical_descending_acceleration = constraints.vertical_descending_acceleration;
+    2016             :       constraints_filtered_.vertical_descending_jerk         = constraints.vertical_descending_jerk;
+    2017             :       constraints_filtered_.vertical_descending_snap         = constraints.vertical_descending_snap;
+    2018           0 : 
+    2019             :       constraints_filtered_.heading_acceleration = constraints.heading_acceleration;
+    2020             :       constraints_filtered_.heading_jerk         = constraints.heading_jerk;
+    2021             :       constraints_filtered_.heading_snap         = constraints.heading_snap;
+    2022             :     }
+    2023             : 
+    2024             :     ROS_INFO_THROTTLE(1.0, "[MpcTracker]: all constraints succesfully applied");
+    2025             :     all_constraints_set_ = true;
+    2026       86608 : 
+    2027             :   } else {
+    2028       86608 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: slowing down to apply new constraints");
+    2029             :   }
+    2030       86608 : }
+    2031             : 
+    2032       86608 : //}
+    2033             : 
+    2034       86608 : /* //{ calculateMPC() */
+    2035       86608 : 
+    2036       86608 : void MpcTracker::calculateMPC() {
+    2037       86608 : 
+    2038             :   std::scoped_lock lock(mutex_mpc_calculation_);
+    2039       86608 : 
+    2040             :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2041      173216 : 
+    2042             :   ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: MPC calculation dt = " << dt1);
+    2043       86608 : 
+    2044       86608 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_filtered_, constraints_filtered_);
+    2045       86608 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2046       86608 :   auto uav_state              = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2047             :   auto drs_params             = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    2048             : 
+    2049       86608 :   MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    2050       86608 :   {
+    2051             :     std::scoped_lock lock(mutex_des_trajectory_);
+    2052       86608 : 
+    2053             :     des_x_trajectory       = des_x_trajectory_;
+    2054             :     des_y_trajectory       = des_y_trajectory_;
+    2055     3509436 :     des_z_trajectory       = des_z_trajectory_;
+    2056     3423840 :     des_heading_trajectory = des_heading_trajectory_;
+    2057      117717 :   }
+    2058             : 
+    2059             :   int    first_collision_index = INT_MAX;
+    2060             :   double lowest_z              = std::numeric_limits<double>::max();
+    2061             : 
+    2062       85596 :   if (collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    2063             : 
+    2064             :     // determine the lowest point in our trajectory
+    2065             :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2066        1012 :       if (des_z_trajectory_(i, 0) < lowest_z) {
+    2067             :         lowest_z = des_z_trajectory_(i, 0);
+    2068             :       }
+    2069       86608 :     }
+    2070       86608 : 
+    2071       86608 :     // check other drone trajectories for collisions
+    2072       86608 :     minimum_collison_free_altitude_ = checkTrajectoryForCollisions(first_collision_index);
+    2073             : 
+    2074       86608 :   } else {
+    2075       86608 : 
+    2076       86608 :     minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+    2077       86608 :   }
+    2078             : 
+    2079       86608 :   double max_speed_x = constraints.horizontal_speed;
+    2080       86608 :   double max_speed_y = constraints.horizontal_speed;
+    2081       86608 :   double max_speed_z = constraints.vertical_ascending_speed;
+    2082       86608 :   double min_speed_z = constraints.vertical_descending_speed;
+    2083             : 
+    2084       86608 :   double max_acc_x = constraints.horizontal_acceleration;
+    2085       86608 :   double max_acc_y = constraints.horizontal_acceleration;
+    2086       86608 :   double max_acc_z = constraints.vertical_ascending_acceleration;
+    2087       86608 :   double min_acc_z = constraints.vertical_descending_acceleration;
+    2088             : 
+    2089       86608 :   double max_snap_x = constraints.horizontal_snap;
+    2090             :   double max_snap_y = constraints.horizontal_snap;
+    2091       86608 :   double max_snap_z = constraints.vertical_ascending_snap;
+    2092             :   double min_snap_z = constraints.vertical_descending_snap;
+    2093        2578 : 
+    2094             :   double max_jerk_x = constraints.horizontal_jerk;
+    2095        2578 :   double max_jerk_y = constraints.horizontal_jerk;
+    2096             :   double max_jerk_z = constraints.vertical_ascending_jerk;
+    2097        2578 :   double min_jerk_z = constraints.vertical_descending_jerk;
+    2098        2578 : 
+    2099           0 :   collision_avoidance_affecting_me_ = false;
+    2100           0 : 
+    2101           0 :   if (first_collision_index < MPC_HORIZON_LENGTH) {
+    2102           0 : 
+    2103             :     collision_avoidance_affecting_me_ = true;
+    2104             :     // the tmp variable is used to scale the speed of our drone in collision avoidance, depending on how far away the collision is
+    2105        2578 :     double tmp = 0;
+    2106           0 : 
+    2107           0 :     if (first_collision_index <= _avoidance_collision_slow_down_fully_) {
+    2108           0 :       tmp = 1;
+    2109        2578 :     } else if (first_collision_index <= _avoidance_collision_slow_down_) {
+    2110           0 :       tmp = 1.0 - ((double)(first_collision_index - _avoidance_collision_slow_down_fully_)) /
+    2111        2578 :                       (double)(_avoidance_collision_slow_down_ - _avoidance_collision_slow_down_fully_);
+    2112           0 :       tmp = tmp * tmp;
+    2113             :     }
+    2114             : 
+    2115        2578 :     if (!std::isfinite(tmp)) {
+    2116           6 :       tmp = 1.0;
+    2117           6 :       ROS_ERROR("[MpcTracker]: NaN detected in variable 'tmp', setting it to 1.0 and returning!!!");
+    2118             :       return;
+    2119        2578 :     } else if (tmp > 1.0) {
+    2120        2109 :       tmp = 1.0;
+    2121             :     } else if (tmp < 0.0) {
+    2122             :       tmp = 0.0;
+    2123             :     }
+    2124        2578 : 
+    2125        2578 :     if (tmp > coef_scaler) {
+    2126             :       coef_scaler = tmp;
+    2127             :       coef_time   = ros::Time::now();
+    2128       86608 :     }
+    2129             :     if ((ros::Time::now() - coef_time).toSec() > 2.0) {
+    2130        1548 :       coef_scaler = tmp;
+    2131        1548 :     }
+    2132        1548 : 
+    2133             :     // we are close to a possible collision, better slow down a bit to give everyone more time
+    2134             :     max_speed_x = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2135             :     max_speed_y = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2136      173216 :   }
+    2137       86608 : 
+    2138             :   if (collision_free_altitude_ > lowest_z) {
+    2139       86608 : 
+    2140       86608 :     collision_avoidance_affecting_me_ = true;
+    2141       86608 :     max_speed_x                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2142       86608 :     max_speed_y                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2143             :   }
+    2144       86608 : 
+    2145             :   // first control input generated by MPC
+    2146      173216 :   VectorXd mpc_u         = VectorXd::Zero(MPC_N_INPUTS);
+    2147             :   double   mpc_u_heading = 0;
+    2148     3550932 : 
+    2149     3464320 :   double iters_z       = 0;
+    2150       60963 :   double iters_x       = 0;
+    2151             :   double iters_y       = 0;
+    2152     3403357 :   double iters_heading = 0;
+    2153             : 
+    2154             :   ros::Time time_begin = ros::Time::now();
+    2155             : 
+    2156             :   MatrixXd des_z_filtered = filterReferenceZ(des_z_trajectory, max_speed_z, min_speed_z);
+    2157             : 
+    2158             :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2159             :     if (des_z_filtered(i, 0) < minimum_collison_free_altitude_) {
+    2160       86608 :       des_z_filtered_offset_(i, 0) = minimum_collison_free_altitude_;
+    2161       66910 :     } else {
+    2162             :       des_z_filtered_offset_(i, 0) = des_z_filtered(i, 0);
+    2163       19698 :     }
+    2164             :   }
+    2165             : 
+    2166      173216 :   // | ----------------- prepare the references ----------------- |
+    2167             : 
+    2168       86608 :   // | -------------------- MPC solver z-axis ------------------- |
+    2169       86608 : 
+    2170       86608 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2171       86608 :     mpc_solver_z_->setVelQ(drs_params.q_vel_braking);
+    2172             :   } else {
+    2173       86608 :     mpc_solver_z_->setVelQ(drs_params.q_vel_no_braking);
+    2174       86608 :   }
+    2175       86608 : 
+    2176       86608 :   MatrixXd initial_z = MatrixXd::Zero(4, 1);
+    2177       86608 : 
+    2178             :   initial_z(0, 0) = mpc_x(8, 0);
+    2179             :   initial_z(1, 0) = mpc_x(9, 0);
+    2180      173216 :   initial_z(2, 0) = mpc_x(10, 0);
+    2181             :   initial_z(3, 0) = mpc_x(11, 0);
+    2182       86608 : 
+    2183             :   mpc_solver_z_->setDt(dt1);
+    2184             :   mpc_solver_z_->setInitialState(initial_z);
+    2185       86608 :   mpc_solver_z_->loadReference(des_z_filtered_offset_);
+    2186             :   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);
+    2187             :   iters_z += mpc_solver_z_->solveMPC();
+    2188             : 
+    2189             :   {
+    2190       86608 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2191             : 
+    2192       86608 :     mpc_solver_z_->getStates(predicted_trajectory_);
+    2193             :   }
+    2194             : 
+    2195       86608 :   mpc_u(2) = mpc_solver_z_->getFirstControlInput();
+    2196         701 : 
+    2197         701 :   // if we are climbing to avoid a collision, reduce or arrest our horizontal velocity
+    2198             :   double ascend;
+    2199             :   {
+    2200      259824 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2201             : 
+    2202             :     ascend = (predicted_trajectory_(10, 0) / max_speed_z);
+    2203             :   }
+    2204       86608 : 
+    2205             :   if (ascend > 0 && collision_free_altitude_ > lowest_z) {
+    2206     3464320 :     max_speed_y = max_speed_y * (1.0 - ascend);
+    2207     3377708 :     max_speed_x = max_speed_x * (1.0 - ascend);
+    2208             :   }
+    2209             : 
+    2210             :   auto [des_x_filtered, des_y_filtered] = filterReferenceXY(des_x_trajectory, des_y_trajectory, max_speed_x, max_speed_y);
+    2211             : 
+    2212       86608 :   // unwrap the heading reference
+    2213       66907 : 
+    2214             :   des_heading_trajectory(0, 0) = sradians::unwrap(des_heading_trajectory(0, 0), mpc_x_heading(0));
+    2215       19701 : 
+    2216             :   for (int i = 1; i < MPC_HORIZON_LENGTH; i++) {
+    2217             :     des_heading_trajectory(i, 0) = sradians::unwrap(des_heading_trajectory(i, 0), des_heading_trajectory(i - 1, 0));
+    2218      173216 :   }
+    2219             : 
+    2220       86608 :   // | -------------------- MPC solver x-axis ------------------- |
+    2221       86608 : 
+    2222       86608 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2223       86608 :     mpc_solver_x_->setVelQ(drs_params.q_vel_braking);
+    2224             :   } else {
+    2225       86608 :     mpc_solver_x_->setVelQ(drs_params.q_vel_no_braking);
+    2226       86608 :   }
+    2227       86608 : 
+    2228       86608 :   MatrixXd initial_x = MatrixXd::Zero(4, 1);
+    2229       86608 : 
+    2230             :   initial_x(0, 0) = mpc_x(0, 0);
+    2231             :   initial_x(1, 0) = mpc_x(1, 0);
+    2232      173216 :   initial_x(2, 0) = mpc_x(2, 0);
+    2233             :   initial_x(3, 0) = mpc_x(3, 0);
+    2234       86608 : 
+    2235             :   mpc_solver_x_->setDt(dt1);
+    2236             :   mpc_solver_x_->setInitialState(initial_x);
+    2237       86608 :   mpc_solver_x_->loadReference(des_x_filtered);
+    2238             :   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);
+    2239             :   iters_x += mpc_solver_x_->solveMPC();
+    2240             : 
+    2241       86608 :   {
+    2242       66906 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2243             : 
+    2244       19702 :     mpc_solver_x_->getStates(predicted_trajectory_);
+    2245             :   }
+    2246             : 
+    2247      173216 :   mpc_u(0) = mpc_solver_x_->getFirstControlInput();
+    2248             : 
+    2249       86608 :   // | -------------------- MPC solver y-axis ------------------- |
+    2250       86608 : 
+    2251       86608 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2252       86608 :     mpc_solver_y_->setVelQ(drs_params.q_vel_braking);
+    2253             :   } else {
+    2254       86608 :     mpc_solver_y_->setVelQ(drs_params.q_vel_no_braking);
+    2255       86608 :   }
+    2256       86608 : 
+    2257       86608 :   MatrixXd initial_y = MatrixXd::Zero(4, 1);
+    2258       86608 : 
+    2259             :   initial_y(0, 0) = mpc_x(4, 0);
+    2260      173216 :   initial_y(1, 0) = mpc_x(5, 0);
+    2261             :   initial_y(2, 0) = mpc_x(6, 0);
+    2262       86608 :   initial_y(3, 0) = mpc_x(7, 0);
+    2263             : 
+    2264       86608 :   mpc_solver_y_->setDt(dt1);
+    2265             :   mpc_solver_y_->setInitialState(initial_y);
+    2266             :   mpc_solver_y_->loadReference(des_y_filtered);
+    2267             :   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);
+    2268       86608 :   iters_y += mpc_solver_y_->solveMPC();
+    2269       66905 :   {
+    2270             :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2271       19703 : 
+    2272             :     mpc_solver_y_->getStates(predicted_trajectory_);
+    2273             :   }
+    2274       86608 :   mpc_u(1) = mpc_solver_y_->getFirstControlInput();
+    2275       86608 : 
+    2276       86608 :   // | ------------------- MPC solver heading ------------------- |
+    2277       86608 : 
+    2278             :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2279       86608 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_braking);
+    2280             :   } else {
+    2281      173216 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_no_braking);
+    2282             :   }
+    2283       86608 : 
+    2284             :   mpc_solver_heading_->setDt(dt1);
+    2285       86608 :   mpc_solver_heading_->setInitialState(mpc_x_heading);
+    2286             :   mpc_solver_heading_->loadReference(des_heading_trajectory);
+    2287             :   mpc_solver_heading_->setLimits(constraints.heading_speed, constraints.heading_speed, constraints.heading_acceleration, constraints.heading_acceleration,
+    2288       86608 :                                  constraints.heading_jerk, constraints.heading_jerk, constraints.heading_snap, constraints.heading_snap);
+    2289             :   iters_heading += mpc_solver_heading_->solveMPC();
+    2290       86608 :   {
+    2291           0 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2292           0 : 
+    2293           0 :     mpc_solver_heading_->getStates(predicted_heading_trajectory_);
+    2294             :   }
+    2295       86608 :   mpc_u_heading = mpc_solver_heading_->getFirstControlInput();
+    2296           0 : 
+    2297           0 :   {
+    2298           0 :     bool saturating = false;
+    2299             : 
+    2300       86608 :     if (mpc_u(0) > max_snap_x * 1.01) {
+    2301           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2302           0 :       mpc_u(0)   = max_snap_x;
+    2303           0 :       saturating = true;
+    2304             :     }
+    2305       86608 :     if (mpc_u(0) < -max_snap_x * 1.01) {
+    2306           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2307           0 :       mpc_u(0)   = -max_snap_x;
+    2308           0 :       saturating = true;
+    2309             :     }
+    2310       86608 :     if (mpc_u(1) > max_snap_y * 1.01) {
+    2311           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2312           0 :       mpc_u(1)   = max_snap_y;
+    2313           0 :       saturating = true;
+    2314             :     }
+    2315       86608 :     if (mpc_u(1) < -max_snap_y * 1.01) {
+    2316           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2317           0 :       mpc_u(1)   = -max_snap_y;
+    2318           0 :       saturating = true;
+    2319             :     }
+    2320             :     if (mpc_u(2) > max_snap_z * 1.01) {
+    2321       86608 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2322           0 :       mpc_u(2)   = max_snap_z;
+    2323           0 :       saturating = true;
+    2324             :     }
+    2325             :     if (mpc_u(2) < -min_snap_z * 1.01) {
+    2326             :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2327             :       mpc_u(2)   = -min_snap_z;
+    2328       86608 :       saturating = true;
+    2329             :     }
+    2330       86608 : 
+    2331       86608 :     if (saturating) {
+    2332             :       debugPrintState(0.1);
+    2333             :       debugPrintMPCResult(0.1);
+    2334       86608 :     }
+    2335       86608 :   }
+    2336           0 : 
+    2337             :   {
+    2338             :     std::scoped_lock lock(mutex_mpc_u_);
+    2339             : 
+    2340             :     mpc_u_         = mpc_u;
+    2341             :     mpc_u_heading_ = mpc_u_heading;
+    2342       86608 :   }
+    2343             : 
+    2344             :   double mpc_solver_time = (ros::Time::now() - time_begin).toSec();
+    2345             :   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_) {
+    2346      163790 :     ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: Total MPC solver time: " << mpc_solver_time << " iters X: " << iters_x << "/" << _max_iters_xy_
+    2347      149965 :                                                                            << " iters Y:  " << iters_y << "/" << _max_iters_xy_ << " iters Z: " << iters_z
+    2348      245999 :                                                                            << "/" << _max_iters_z_ << " iters heading: " << iters_heading << "/"
+    2349       71139 :                                                                            << _max_iters_heading_);
+    2350             :   }
+    2351       70993 : 
+    2352       70993 :   future_was_predicted_ = true;
+    2353             : 
+    2354             :   // | ------------- breaking for the next iteration ------------ |
+    2355       15615 : 
+    2356             :   if (drs_params.braking_enabled && (std::abs(des_x_filtered(MPC_HORIZON_LENGTH - 6) - des_x_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2357             :       (std::abs(des_y_filtered(MPC_HORIZON_LENGTH - 6) - des_y_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2358             :       (std::abs(des_z_filtered(MPC_HORIZON_LENGTH - 6) - des_z_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2359             :       (std::abs(radians::diff(des_heading_trajectory(MPC_HORIZON_LENGTH - 6), des_heading_trajectory(MPC_HORIZON_LENGTH - 1))) <= 0.1)) {
+    2360             : 
+    2361      173216 :     brake_ = true;
+    2362       86608 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: braking");
+    2363       86608 : 
+    2364             :   } else {
+    2365             :     brake_ = false;
+    2366      173216 :   }
+    2367             : 
+    2368     3550932 :   /* publish mpc reference //{ */
+    2369             : 
+    2370     3464320 :   {
+    2371             :     geometry_msgs::PoseArray debug_trajectory_out;
+    2372     3464320 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2373     3464320 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    2374     3464320 : 
+    2375             :     {
+    2376     3464320 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    2377             : 
+    2378     3464320 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2379             : 
+    2380             :         geometry_msgs::Pose new_pose;
+    2381             : 
+    2382       86608 :         new_pose.position.x = des_x_filtered(i, 0);
+    2383             :         new_pose.position.y = des_y_filtered(i, 0);
+    2384             :         new_pose.position.z = des_z_filtered(i, 0);
+    2385             : 
+    2386             :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(i));
+    2387             : 
+    2388             :         debug_trajectory_out.poses.push_back(new_pose);
+    2389             :       }
+    2390             :     }
+    2391             : 
+    2392       79929 :     ph_mpc_reference_debugging_.publish(debug_trajectory_out);
+    2393             :   }
+    2394       79929 : 
+    2395             :   //}
+    2396       79929 : }
+    2397             : 
+    2398          98 : //}
+    2399          98 : 
+    2400             : /* iterateModel() //{ */
+    2401             : 
+    2402             : void MpcTracker::iterateModel(const double& dt) {
+    2403       79831 : 
+    2404             :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2405       79831 : 
+    2406       79831 :   if (model_first_iteration_) {
+    2407             : 
+    2408             :     model_iteration_last_time_ = ros::Time::now();
+    2409       79831 :     model_first_iteration_     = false;
+    2410       79831 : 
+    2411       79831 :   } else {
+    2412       79831 : 
+    2413       79831 :     dt1 = 0.9 * dt1 + 0.1 * dt;
+    2414       79831 : 
+    2415       79831 :     mrs_lib::set_mutexed(mutex_dt1_, dt1, dt1_);
+    2416       79831 :     timer_mpc_iteration_.setPeriod(ros::Duration(dt1), false);
+    2417       79831 : 
+    2418       79831 :     // clang-format off
+    2419       79831 :     A_ << 1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,           0, 0,   0,           0,
+    2420       79831 :           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,           0, 0,   0,           0,
+    2421             :           0, 0,   1,           dt1,         0, 0,   0,           0,           0, 0,   0,           0,
+    2422       79831 :           0, 0,   0,           1,           0, 0,   0,           0,           0, 0,   0,           0,
+    2423       79831 :           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,
+    2424       79831 :           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,
+    2425       79831 :           0, 0,   0,           0,           0, 0,   1,           dt1,         0, 0,   0,           0,
+    2426       79831 :           0, 0,   0,           0,           0, 0,   0,           1,           0, 0,   0,           0,
+    2427       79831 :           0, 0,   0,           0,           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,
+    2428       79831 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1,
+    2429       79831 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   1,           dt1,
+    2430       79831 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   0,           1;
+    2431       79831 : 
+    2432       79831 :       B_ << 0,   0,   0,
+    2433       79831 :             0,   0,   0,
+    2434             :             0,   0,   0,
+    2435       79831 :             dt1, 0,   0,
+    2436       79831 :             0,   0,   0,
+    2437       79831 :             0,   0,   0,
+    2438       79831 :             0,   0,   0,
+    2439             :             0,   dt1, 0,
+    2440       79831 :             0,   0,   0,
+    2441       79831 :             0,   0,   0,
+    2442      159662 :             0,   0,   0,
+    2443       79831 :             0,   0,   dt1;
+    2444             : 
+    2445       79831 :       A_heading_ << 1, dt1, 0.5*dt1*dt1, 0,
+    2446             :                     0, 1,   dt1,         0.5*dt1*dt1,
+    2447             :                     0, 0,   1,           dt1,
+    2448             :                     0, 0,   0,           1;
+    2449      159858 : 
+    2450      159858 :       B_heading_ << 0,
+    2451             :                     0,
+    2452      159858 :                     0,
+    2453      159858 :                     dt1;
+    2454             : 
+    2455             :     model_iteration_last_time_ = ros::Time::now();
+    2456             :   }
+    2457       79929 : 
+    2458             :   {
+    2459       79929 :     auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2460             :     auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    2461             : 
+    2462             :     MatrixXd new_mpc_x         = A_ * mpc_x + B_ * mpc_u;
+    2463       79929 :     MatrixXd new_mpc_x_heading = A_heading_ * mpc_x_heading + B_heading_ * mpc_u_heading;
+    2464           0 : 
+    2465             :     // | --------------- check the state difference --------------- |
+    2466           0 :     {
+    2467             :       auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    2468             : 
+    2469       79929 :       bool problem = false;
+    2470           0 : 
+    2471             :       // position
+    2472           0 : 
+    2473             :       if (fabs((new_mpc_x(0) - mpc_x(0)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2474             :         ROS_DEBUG("[MpcTracker]: horizontal pos x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(0), new_mpc_x(0),
+    2475       79929 :                   fabs((new_mpc_x(0) - mpc_x(0)) / dt1), constraints.horizontal_speed);
+    2476           0 :         problem = true;
+    2477             :       }
+    2478           0 : 
+    2479             :       if (fabs((new_mpc_x(4) - mpc_x(4)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2480             :         ROS_DEBUG("[MpcTracker]: horizontal pos y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(4), new_mpc_x(4),
+    2481       79929 :                   fabs((new_mpc_x(4) - mpc_x(4)) / dt1), constraints.horizontal_speed);
+    2482           0 :         problem = true;
+    2483             :     }
+    2484           0 : 
+    2485             :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) > 1.05 * constraints.vertical_ascending_speed) {
+    2486             :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(8), new_mpc_x(8),
+    2487             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), constraints.vertical_ascending_speed);
+    2488             :         problem = true;
+    2489             :       }
+    2490             : 
+    2491             :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) < 1.05 * -constraints.vertical_descending_speed) {
+    2492             :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(8), new_mpc_x(8),
+    2493             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), -constraints.vertical_descending_speed);
+    2494             :         problem = true;
+    2495       79929 :       }
+    2496           0 : 
+    2497             :       /* if (fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1) > 1.2 * constraints.heading_speed) { */
+    2498           0 :       /*   ROS_DEBUG("[MpcTracker]: heading update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x_heading(0), new_mpc_x_heading(0), */
+    2499             :       /*             fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1), constraints.heading_speed); */
+    2500             :       /*   problem = true; */
+    2501       79929 :       /* } */
+    2502           0 : 
+    2503             :       // velocity
+    2504           0 : 
+    2505             :       if (fabs((new_mpc_x(1) - mpc_x(1)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2506             :         ROS_DEBUG("[MpcTracker]: horizontal vel x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(1), new_mpc_x(1),
+    2507       79929 :                   fabs((new_mpc_x(1) - mpc_x(1)) / dt1), constraints.horizontal_acceleration);
+    2508           0 :         problem = true;
+    2509             :       }
+    2510           0 : 
+    2511             :       if (fabs((new_mpc_x(5) - mpc_x(5)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2512             :         ROS_DEBUG("[MpcTracker]: horizontal vel y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(5), new_mpc_x(5),
+    2513       79929 :                   fabs((new_mpc_x(5) - mpc_x(5)) / dt1), constraints.horizontal_acceleration);
+    2514           0 :         problem = true;
+    2515             :       }
+    2516           0 : 
+    2517             :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) > 1.05 * constraints.vertical_ascending_acceleration) {
+    2518             :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(9), new_mpc_x(9),
+    2519             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), constraints.vertical_ascending_acceleration);
+    2520             :         problem = true;
+    2521       79929 :       }
+    2522           0 : 
+    2523             :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) < 1.05 * -constraints.vertical_descending_acceleration) {
+    2524           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(9), new_mpc_x(9),
+    2525             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), -constraints.vertical_descending_acceleration);
+    2526             :         problem = true;
+    2527       79929 :       }
+    2528           0 : 
+    2529             :       // acceleration
+    2530           0 : 
+    2531             :       if (fabs((new_mpc_x(2) - mpc_x(2)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2532             :         ROS_DEBUG("[MpcTracker]: horizontal acc x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(2), new_mpc_x(2),
+    2533       79929 :                   fabs((new_mpc_x(2) - mpc_x(2)) / dt1), constraints.horizontal_jerk);
+    2534           0 :         problem = true;
+    2535             :       }
+    2536           0 : 
+    2537             :       if (fabs((new_mpc_x(6) - mpc_x(6)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2538             :         ROS_DEBUG("[MpcTracker]: horizontal acc y update violates constraints: %.2f -> %.2f, = %.2f > %.2f", mpc_x(6), new_mpc_x(6),
+    2539       79929 :                   fabs((new_mpc_x(6) - mpc_x(6)) / dt1), constraints.horizontal_jerk);
+    2540           0 :         problem = true;
+    2541             :       }
+    2542           0 : 
+    2543             :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) > 1.05 * constraints.vertical_ascending_jerk) {
+    2544             :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(10), new_mpc_x(10),
+    2545             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), constraints.vertical_ascending_jerk);
+    2546             :         problem = true;
+    2547       79929 :       }
+    2548           0 : 
+    2549             :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) < 1.05 * -constraints.vertical_descending_jerk) {
+    2550           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(10), new_mpc_x(10),
+    2551             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), -constraints.vertical_descending_jerk);
+    2552             :         problem = true;
+    2553       79929 :       }
+    2554           0 : 
+    2555             :       // jerk
+    2556           0 : 
+    2557             :       if (fabs((new_mpc_x(3) - mpc_x(3)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2558             :         ROS_DEBUG("[MpcTracker]: horizontal jerk x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(3), new_mpc_x(3),
+    2559       79929 :                   fabs((new_mpc_x(3) - mpc_x(3)) / dt1), constraints.horizontal_snap);
+    2560           0 :         problem = true;
+    2561             :       }
+    2562           0 : 
+    2563             :       if (fabs((new_mpc_x(7) - mpc_x(7)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2564             :         ROS_DEBUG("[MpcTracker]: horizontal jerk y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(7), new_mpc_x(7),
+    2565       79929 :                   fabs((new_mpc_x(7) - mpc_x(7)) / dt1), constraints.horizontal_snap);
+    2566           0 :         problem = true;
+    2567             :       }
+    2568           0 : 
+    2569             :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) > 1.05 * constraints.vertical_ascending_snap) {
+    2570             :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(11), new_mpc_x(11),
+    2571       79929 :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), constraints.vertical_ascending_snap);
+    2572           0 :         problem = true;
+    2573           0 :       }
+    2574             : 
+    2575             :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) < 1.05 * -constraints.vertical_descending_snap) {
+    2576             :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(11), new_mpc_x(11),
+    2577             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), -constraints.vertical_descending_snap);
+    2578       79929 :         problem = true;
+    2579             :       }
+    2580       79929 : 
+    2581       79929 :       if (problem) {
+    2582             :         debugPrintState(0.001);
+    2583       79929 :         debugPrintMPCResult(0.001);
+    2584             :       }
+    2585             :     }
+    2586       79929 : 
+    2587             :     {
+    2588             :       std::scoped_lock lock(mutex_mpc_x_);
+    2589             : 
+    2590             :       mpc_x_         = new_mpc_x;
+    2591             :       mpc_x_heading_ = new_mpc_x_heading;
+    2592             : 
+    2593             :       mpc_x_heading_(0) = sradians::wrap(mpc_x_heading_(0));
+    2594             :     }
+    2595         742 :   }
+    2596             : }
+    2597         742 : 
+    2598             : //}
+    2599             : 
+    2600        1484 : // | -------------------- referece setting -------------------- |
+    2601        1484 : 
+    2602             : /* //{ loadTrajectory() */
+    2603        1484 : 
+    2604             : // method for setting desired trajectory
+    2605             : std::tuple<bool, std::string, bool> MpcTracker::loadTrajectory(const mrs_msgs::TrajectoryReference msg) {
+    2606             : 
+    2607             :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2608         742 : 
+    2609           0 :   // copy the member variables
+    2610           0 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    2611         742 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2612           0 : 
+    2613           0 :   std::stringstream ss;
+    2614           0 : 
+    2615           0 :   /* check the trajectory dt //{ */
+    2616             : 
+    2617         742 :   double trajectory_dt;
+    2618             :   if (msg.dt <= 1e-4) {
+    2619             :     trajectory_dt = 0.2;
+    2620             :     ROS_WARN_THROTTLE(10.0, "[MpcTracker]: the trajectory dt was not specified, assuming its the old 0.2 s");
+    2621             :   } else if (msg.dt < dt1) {
+    2622         742 :     trajectory_dt = 0.2;
+    2623             :     ss << std::setprecision(3) << "the trajectory dt (" << msg.dt << " s) is too small (smaller than the tracker's internal step size: " << dt1 << " s)";
+    2624             :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2625             :     return std::tuple(false, ss.str(), false);
+    2626         742 :   } else {
+    2627         742 :     trajectory_dt = msg.dt;
+    2628         742 :   }
+    2629             : 
+    2630             :   //}
+    2631         742 : 
+    2632             :   int trajectory_size = msg.points.size();
+    2633         740 : 
+    2634             :   /* sanitize the time-ness of the trajectory //{ */
+    2635             : 
+    2636             :   int    trajectory_sample_offset    = 0;  // how many samples in past is the trajectory
+    2637         740 :   int    trajectory_subsample_offset = 0;  // how many simulation inner loops ahead of the first valid sample
+    2638             :   double trajectory_time_offset      = 0;  // how much time in past in [s]
+    2639           3 : 
+    2640             :   // btw, "trajectory_time_offset = trajectory_dt*trajectory_sample_offset + _dt1_*trajectory_subsample_offset" should hold
+    2641             :   if (msg.fly_now) {
+    2642             : 
+    2643             :     ros::Time trajectory_time = msg.header.stamp;
+    2644         737 : 
+    2645             :     // the desired time is 0 => the current time
+    2646             :     // the trajecoty is a single point => the current time
+    2647             :     if (trajectory_time == ros::Time(0) || int(msg.points.size()) == 1) {
+    2648         737 : 
+    2649             :       trajectory_time_offset = 0.0;
+    2650           0 : 
+    2651             :       // the desired time is specified
+    2652           0 :     } else {
+    2653             : 
+    2654             :       trajectory_time_offset = (ros::Time::now() - trajectory_time).toSec();
+    2655             : 
+    2656             :       // when the time offset is negative, thus in the future
+    2657         740 :       // just say it, but use it like its from the current time
+    2658             :       if (trajectory_time_offset < 0.0) {
+    2659             : 
+    2660           1 :         ROS_WARN_THROTTLE(1.0, "[MpcTracker]: received trajectory with timestamp in the future by %.3f s", -trajectory_time_offset);
+    2661             : 
+    2662             :         trajectory_time_offset = 0.0;
+    2663           1 :       }
+    2664             :     }
+    2665           1 : 
+    2666             :     // if the time offset is set, check if we need to "move the first idx"
+    2667             :     if (trajectory_time_offset > 0.0) {
+    2668             : 
+    2669             :       // calculate the offset in samples
+    2670           1 :       trajectory_sample_offset = int(floor(trajectory_time_offset / trajectory_dt));
+    2671             : 
+    2672           0 :       // and get the subsample offset, which will be used to initialize the interpolator
+    2673           0 :       trajectory_subsample_offset = int(floor(fmod(trajectory_time_offset, trajectory_dt) / dt1));
+    2674           0 : 
+    2675             :       ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: received trajectory with timestamp in the past by %.3f s",
+    2676             :                          trajectory_dt * trajectory_sample_offset + dt1 * trajectory_subsample_offset);
+    2677             : 
+    2678             :       // if the offset is larger than the number of points in the trajectory
+    2679             :       // the trajectory can not be used
+    2680           1 :       if (trajectory_sample_offset >= trajectory_size) {
+    2681             : 
+    2682             :         ss << "trajectory timestamp is too old (time difference = " << trajectory_time_offset << ")";
+    2683           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2684             :         return std::tuple(false, ss.str(), false);
+    2685           0 : 
+    2686             :       } else {
+    2687             : 
+    2688             :         // if the offset is larger than one trajectory sample,
+    2689           1 :         // offset the start
+    2690             :         if (trajectory_time_offset >= trajectory_dt) {
+    2691             : 
+    2692             :           // decrease the trajectory size
+    2693             :           trajectory_size -= trajectory_sample_offset;
+    2694             : 
+    2695             :           ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: offsetting trajectory by %d samples", trajectory_sample_offset);
+    2696           2 : 
+    2697             :         } else {
+    2698             : 
+    2699             :           trajectory_sample_offset = 0;
+    2700             :         }
+    2701         742 :       }
+    2702         742 :     }
+    2703             : 
+    2704             :   } else { // not fly_now
+    2705             : 
+    2706             :       trajectory_tracking_in_progress_ = false;
+    2707             :   }
+    2708             : 
+    2709             :   //}
+    2710             : 
+    2711             :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory sample offset: %d", trajectory_sample_offset);
+    2712             :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory subsample offset: %d", trajectory_subsample_offset);
+    2713        1484 : 
+    2714        1484 :   // after this, we should have the correct value of
+    2715        1484 :   // * trajectory_size
+    2716        1484 :   // * trajectory_sample_offset
+    2717             :   // * trajectory_subsample_offset
+    2718       38179 : 
+    2719             :   /* copy the trajectory to a local variable //{ */
+    2720       37437 : 
+    2721       37437 :   // copy only the part from the first valid index
+    2722       37437 : 
+    2723       37437 :   MatrixXd des_x_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2724             :   MatrixXd des_y_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2725             :   MatrixXd des_z_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2726             :   MatrixXd des_heading_whole_trajectory = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2727             : 
+    2728             :   for (int i = 0; i < trajectory_size; i++) {
+    2729             : 
+    2730         742 :     des_x_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.x;
+    2731             :     des_y_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.y;
+    2732         742 :     des_z_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.z;
+    2733             :     des_heading_whole_trajectory(i) = msg.points.at(trajectory_sample_offset + i).heading;
+    2734           0 :   }
+    2735           0 : 
+    2736           0 :   //}
+    2737           0 : 
+    2738             :   /* set looping //{ */
+    2739           0 : 
+    2740           0 :   bool loop = false;
+    2741           0 : 
+    2742           0 :   if (msg.loop) {
+    2743             : 
+    2744             :     double first_x = des_x_whole_trajectory(0);
+    2745           0 :     double first_y = des_y_whole_trajectory(0);
+    2746             :     double first_z = des_z_whole_trajectory(0);
+    2747           0 :     double first_hdg = des_heading_whole_trajectory(0);
+    2748           0 : 
+    2749             :     double last_x = des_x_whole_trajectory(trajectory_size - 1);
+    2750             :     double last_y = des_y_whole_trajectory(trajectory_size - 1);
+    2751             :     double last_z = des_z_whole_trajectory(trajectory_size - 1);
+    2752           0 :     double last_hdg = des_heading_whole_trajectory(trajectory_size - 1);
+    2753           0 : 
+    2754           0 :     // check whether the trajectory is loopable
+    2755             :     if (mrs_lib::geometry::dist(vec3_t(first_x, first_y, first_z), vec3_t(last_x, last_y, last_z)) < 1.0 && abs(sradians::diff(first_hdg, last_hdg)) < 0.2) {
+    2756             : 
+    2757             :       ROS_INFO_THROTTLE(1.0, "[MpcTracker]: looping enabled");
+    2758             :       loop = true;
+    2759         742 : 
+    2760             :     } else {
+    2761             : 
+    2762             :       ss << "can not loop trajectory, the first and last points are too far apart";
+    2763             :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2764             :       return std::tuple(false, ss.str(), false);
+    2765             :     }
+    2766             : 
+    2767             :   } else {
+    2768             : 
+    2769         742 :     loop = false;
+    2770             :   }
+    2771             : 
+    2772       30422 :   //}
+    2773             : 
+    2774       29680 :   // by this time, the values of these should be set:
+    2775       29680 :   // * loop
+    2776       29680 : 
+    2777       29680 :   /* add tail (the last point repeated to fill the prediction horizon) //{ */
+    2778             : 
+    2779             :   if (!loop) {
+    2780             : 
+    2781             :     // extend it so it has smooth ending
+    2782             :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2783             : 
+    2784             :       des_x_whole_trajectory(i + trajectory_size)       = des_x_whole_trajectory(i + trajectory_size - 1);
+    2785             :       des_y_whole_trajectory(i + trajectory_size)       = des_y_whole_trajectory(i + trajectory_size - 1);
+    2786             :       des_z_whole_trajectory(i + trajectory_size)       = des_z_whole_trajectory(i + trajectory_size - 1);
+    2787             :       des_heading_whole_trajectory(i + trajectory_size) = des_heading_whole_trajectory(i + trajectory_size - 1);
+    2788             :     }
+    2789             :   }
+    2790             : 
+    2791             :   //}
+    2792             : 
+    2793        1484 :   // by this time, the values of these should be set correctly:
+    2794             :   // * trajectory_size
+    2795         742 :   // * des_x_whole_trajectory
+    2796             :   // * des_y_whole_trajectory
+    2797         742 :   // * des_z_whole_trajectory
+    2798             :   // * des_heading_whole_trajectory
+    2799         742 : 
+    2800         742 :   /* update the global variables //{ */
+    2801             : 
+    2802             :   {
+    2803         742 :     std::scoped_lock lock(mutex_des_whole_trajectory_, mutex_des_trajectory_, mutex_trajectory_tracking_states_);
+    2804         742 : 
+    2805         742 :     des_whole_trajectory_id_ = msg.input_id;
+    2806         742 : 
+    2807             :     auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2808       67859 : 
+    2809             :     trajectory_tracking_in_progress_ = msg.fly_now;
+    2810       67117 :     trajectory_track_heading_        = msg.use_heading;
+    2811       67117 : 
+    2812       67117 :     // allocate the vectors
+    2813             :     des_x_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2814       67117 :     des_y_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2815       67117 :     des_z_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2816             :     des_heading_whole_trajectory_ = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2817           0 : 
+    2818             :     for (int i = 0; i < trajectory_size + MPC_HORIZON_LENGTH; i++) {
+    2819             : 
+    2820             :       (*des_x_whole_trajectory_)(i) = des_x_whole_trajectory(i);
+    2821             :       (*des_y_whole_trajectory_)(i) = des_y_whole_trajectory(i);
+    2822         742 :       (*des_z_whole_trajectory_)(i) = des_z_whole_trajectory(i);
+    2823             : 
+    2824         740 :       if (trajectory_track_heading_) {
+    2825             :         (*des_heading_whole_trajectory_)(i) = des_heading_whole_trajectory(i);
+    2826             :       } else {
+    2827             :         (*des_heading_whole_trajectory_).fill(mpc_x_heading(0, 0));
+    2828       30340 :       }
+    2829             :     }
+    2830       29600 : 
+    2831             :     // if we are tracking trajectory, copy the setpoint
+    2832       29600 :     if (trajectory_tracking_in_progress_) {
+    2833       29600 : 
+    2834             :       toggleHover(false);
+    2835       29600 : 
+    2836             :       /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    2837       29600 : 
+    2838             :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2839           0 : 
+    2840           0 :         double first_time = dt1 + i * _dt2_ + trajectory_subsample_offset * dt1;
+    2841             : 
+    2842             :         int first_idx  = floor(first_time / trajectory_dt);
+    2843           0 :         int second_idx = first_idx + 1;
+    2844           0 : 
+    2845             :         double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    2846             : 
+    2847             :         if (trajectory_tracking_loop_) {
+    2848       29600 : 
+    2849           5 :           if (second_idx >= trajectory_size) {
+    2850             :             second_idx -= trajectory_size;
+    2851             :           }
+    2852       29600 : 
+    2853           4 :           if (first_idx >= trajectory_size) {
+    2854             :             first_idx -= trajectory_size;
+    2855             :           }
+    2856             :         } else {
+    2857       29600 : 
+    2858       29600 :           if (second_idx >= trajectory_size) {
+    2859       29600 :             second_idx = trajectory_size - 1;
+    2860             :           }
+    2861       29600 : 
+    2862             :           if (first_idx >= trajectory_size) {
+    2863             :             first_idx = trajectory_size - 1;
+    2864             :           }
+    2865             :         }
+    2866             : 
+    2867         742 :         des_x_trajectory_(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    2868         742 :         des_y_trajectory_(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    2869         742 :         des_z_trajectory_(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    2870         742 : 
+    2871         742 :         des_heading_trajectory_(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    2872         742 :       }
+    2873             : 
+    2874             :       //}
+    2875             :     }
+    2876             : 
+    2877         742 :     trajectory_size_             = trajectory_size;
+    2878             :     trajectory_current_time_     = 0;
+    2879             :     trajectory_set_              = true;
+    2880             :     trajectory_tracking_loop_    = loop;
+    2881             :     trajectory_dt_               = trajectory_dt;
+    2882             :     trajectory_count_++;
+    2883        1484 :   }
+    2884         742 : 
+    2885         742 :   //}
+    2886             : 
+    2887             :   ROS_INFO_THROTTLE(1, "[MpcTracker]: finished setting trajectory with length %d", trajectory_size);
+    2888        1484 : 
+    2889             :   /* publish the debugging topics of the post-processed trajectory //{ */
+    2890       38179 : 
+    2891             :   {
+    2892       37437 : 
+    2893             :     geometry_msgs::PoseArray debug_trajectory_out;
+    2894       37437 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2895       37437 :     debug_trajectory_out.header.frame_id = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2896       37437 : 
+    2897             :     {
+    2898       37437 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2899             : 
+    2900       37437 :       for (int i = 0; i < trajectory_size; i++) {
+    2901             : 
+    2902             :         geometry_msgs::Pose new_pose;
+    2903             : 
+    2904         742 :         new_pose.position.x = (*des_x_whole_trajectory_)(i);
+    2905             :         new_pose.position.y = (*des_y_whole_trajectory_)(i);
+    2906        1484 :         new_pose.position.z = (*des_z_whole_trajectory_)(i);
+    2907             : 
+    2908        1484 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(i));
+    2909             : 
+    2910         742 :         debug_trajectory_out.poses.push_back(new_pose);
+    2911         742 :       }
+    2912         742 :     }
+    2913         742 : 
+    2914         742 :     pub_debug_processed_trajectory_poses_.publish(debug_trajectory_out);
+    2915         742 : 
+    2916         742 :     visualization_msgs::MarkerArray msg_out;
+    2917         742 : 
+    2918         742 :     visualization_msgs::Marker marker;
+    2919             : 
+    2920             :     marker.header.stamp     = ros::Time::now();
+    2921        1484 :     marker.header.frame_id  = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2922             :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    2923       37437 :     marker.color.a          = 1;
+    2924             :     marker.scale.x          = 0.05;
+    2925       36695 :     marker.color.r          = 1;
+    2926             :     marker.color.g          = 0;
+    2927       36695 :     marker.color.b          = 0;
+    2928       36695 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2929       36695 : 
+    2930             :     {
+    2931       36695 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2932             : 
+    2933       36695 :       for (int i = 0; i < trajectory_size - 1; i++) {
+    2934             : 
+    2935       36695 :         geometry_msgs::Point point1;
+    2936       36695 : 
+    2937       36695 :         point1.x = des_x_whole_trajectory(i);
+    2938             :         point1.y = des_y_whole_trajectory(i);
+    2939       36695 :         point1.z = des_z_whole_trajectory(i);
+    2940             : 
+    2941             :         marker.points.push_back(point1);
+    2942             : 
+    2943         742 :         geometry_msgs::Point point2;
+    2944             : 
+    2945         742 :         point2.x = des_x_whole_trajectory(i + 1);
+    2946             :         point2.y = des_y_whole_trajectory(i + 1);
+    2947             :         point2.z = des_z_whole_trajectory(i + 1);
+    2948             : 
+    2949             :         marker.points.push_back(point2);
+    2950         742 :       }
+    2951             :     }
+    2952        1484 : 
+    2953             :     msg_out.markers.push_back(marker);
+    2954             : 
+    2955             :     pub_debug_processed_trajectory_markers_.publish(msg_out);
+    2956             :   }
+    2957             : 
+    2958             :   //}
+    2959             : 
+    2960         531 :   publishDiagnostics();
+    2961             : 
+    2962        1062 :   return std::tuple(true, "trajectory loaded", false);
+    2963             : }
+    2964         531 : 
+    2965         531 : //}
+    2966         531 : 
+    2967         531 : /* //{ setSinglePointReference() */
+    2968         531 : 
+    2969             : // fill the des_*_trajectory based on a single point
+    2970             : void MpcTracker::setSinglePointReference(const double x, const double y, const double z, const double heading) {
+    2971             : 
+    2972             :   std::scoped_lock lock(mutex_des_trajectory_);
+    2973             : 
+    2974             :   des_x_trajectory_.fill(x);
+    2975         262 :   des_y_trajectory_.fill(y);
+    2976             :   des_z_trajectory_.fill(z);
+    2977         262 :   des_heading_trajectory_.fill(heading);
+    2978             : }
+    2979         524 : 
+    2980             : //}
+    2981         262 : 
+    2982           0 : /* //{ setGoal() */
+    2983             : 
+    2984             : // set absolute goal
+    2985         262 : void MpcTracker::setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2986             : 
+    2987         262 :   double desired_heading = sradians::wrap(heading);
+    2988             : 
+    2989         262 :   auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2990         262 : 
+    2991             :   if (!use_heading) {
+    2992             :     desired_heading = mpc_x_heading(0, 0);
+    2993             :   }
+    2994             : 
+    2995             :   trajectory_tracking_in_progress_ = false;
+    2996         269 : 
+    2997             :   setSinglePointReference(pos_x, pos_y, pos_z, desired_heading);
+    2998         538 : 
+    2999             :   publishDiagnostics();
+    3000         269 : }
+    3001         269 : 
+    3002         269 : //}
+    3003             : 
+    3004         269 : /* //{ setRelativeGoal() */
+    3005             : 
+    3006         269 : void MpcTracker::setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    3007           0 : 
+    3008             :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3009             : 
+    3010         269 :   double abs_x = mpc_x(0, 0) + pos_x;
+    3011             :   double abs_y = mpc_x(4, 0) + pos_y;
+    3012         269 :   double abs_z = mpc_x(8, 0) + pos_z;
+    3013             : 
+    3014         269 :   double abs_heading = mpc_x_heading(0, 0);
+    3015         269 : 
+    3016             :   if (use_heading) {
+    3017             :     abs_heading += heading;
+    3018             :   }
+    3019             : 
+    3020             :   trajectory_tracking_in_progress_ = false;
+    3021        1226 : 
+    3022             :   setSinglePointReference(abs_x, abs_y, abs_z, abs_heading);
+    3023        1226 : 
+    3024             :   publishDiagnostics();
+    3025         103 : }
+    3026             : 
+    3027         103 : //}
+    3028             : 
+    3029         103 : /* toggleHover() //{ */
+    3030             : 
+    3031        1123 : void MpcTracker::toggleHover(bool in) {
+    3032             : 
+    3033         103 :   if (in == false && hovering_in_progress_) {
+    3034             : 
+    3035         103 :     ROS_DEBUG("[MpcTracker]: stoppping the hover timer");
+    3036             : 
+    3037         103 :     timer_hover_.stop();
+    3038             : 
+    3039        1226 :     hovering_in_progress_ = false;
+    3040             : 
+    3041             :   } else if (in == true && !hovering_in_progress_) {
+    3042             : 
+    3043             :     ROS_DEBUG("[MpcTracker]: starting the hover timer");
+    3044             : 
+    3045             :     hovering_in_progress_ = true;
+    3046             : 
+    3047           2 :     timer_hover_.start();
+    3048             :   }
+    3049           4 : }
+    3050             : 
+    3051           2 : //}
+    3052             : 
+    3053           2 : // | ------------------- trajectory tracking ------------------ |
+    3054             : 
+    3055             : /* startTrajectoryTrackingImpl() //{ */
+    3056           2 : 
+    3057             : std::tuple<bool, std::string> MpcTracker::startTrajectoryTrackingImpl(void) {
+    3058           2 : 
+    3059           2 :   std::stringstream ss;
+    3060             : 
+    3061             :   if (trajectory_set_) {
+    3062           2 : 
+    3063             :     toggleHover(false);
+    3064           2 : 
+    3065           2 :     {
+    3066             :       std::scoped_lock lock(mutex_des_trajectory_);
+    3067           2 : 
+    3068             :       trajectory_tracking_in_progress_ = true;
+    3069             :       trajectory_current_time_ = 0;
+    3070             :     }
+    3071           0 : 
+    3072           0 :     publishDiagnostics();
+    3073             : 
+    3074           0 :     ss << "trajectory tracking started";
+    3075             :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3076             : 
+    3077             :     return std::tuple(true, ss.str());
+    3078             : 
+    3079             :   } else {
+    3080             : 
+    3081             :     ss << "can not start trajectory tracking, the trajectory is not set";
+    3082           1 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3083             : 
+    3084           2 :     return std::tuple(false, ss.str());
+    3085             :   }
+    3086           1 : }
+    3087             : 
+    3088           1 : //}
+    3089             : 
+    3090           1 : /* resumeTrajectoryTrackingImpl() //{ */
+    3091             : 
+    3092           1 : std::tuple<bool, std::string> MpcTracker::resumeTrajectoryTrackingImpl(void) {
+    3093             : 
+    3094             :   std::stringstream ss;
+    3095           2 : 
+    3096             :   if (trajectory_set_) {
+    3097           1 : 
+    3098             :     toggleHover(false);
+    3099             : 
+    3100           1 :     int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    3101           1 : 
+    3102             :     if (trajectory_tracking_idx < (trajectory_size_ - 1)) {
+    3103           1 : 
+    3104             :       {
+    3105           1 :         std::scoped_lock lock(mutex_des_trajectory_);
+    3106             : 
+    3107             :         trajectory_tracking_in_progress_ = true;
+    3108             :       }
+    3109           0 : 
+    3110           0 :       ss << "trajectory tracking resumed";
+    3111             :       ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3112           0 : 
+    3113             :       publishDiagnostics();
+    3114             : 
+    3115             :       return std::tuple(true, ss.str());
+    3116             : 
+    3117           0 :     } else {
+    3118           0 : 
+    3119             :       ss << "can not resume trajectory tracking, trajectory is already finished";
+    3120           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3121             : 
+    3122             :       return std::tuple(false, ss.str());
+    3123             :     }
+    3124             : 
+    3125             :   } else {
+    3126             : 
+    3127             :     ss << "can not resume trajectory tracking, ther trajectory is not set";
+    3128           1 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3129             : 
+    3130           1 :     return std::tuple(false, ss.str());
+    3131             :   }
+    3132           1 : }
+    3133             : 
+    3134           1 : //}
+    3135             : 
+    3136           1 : /* stopTrajectoryTrackingImpl() //{ */
+    3137             : 
+    3138           1 : std::tuple<bool, std::string> MpcTracker::stopTrajectoryTrackingImpl(void) {
+    3139           1 : 
+    3140             :   std::stringstream ss;
+    3141           1 : 
+    3142             :   if (trajectory_tracking_in_progress_) {
+    3143             : 
+    3144             :     trajectory_tracking_in_progress_ = false;
+    3145           0 : 
+    3146           0 :     toggleHover(true);
+    3147             : 
+    3148             :     ss << "stopping trajectory tracking";
+    3149           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3150             : 
+    3151             :     publishDiagnostics();
+    3152             : 
+    3153             :   } else {
+    3154             : 
+    3155             :     ss << "can not stop trajectory tracking, already at stop";
+    3156           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3157             :   }
+    3158           4 : 
+    3159             :   return std::tuple(true, ss.str());
+    3160           2 : }
+    3161             : 
+    3162           2 : //}
+    3163             : 
+    3164           2 : /* gotoTrajectoryStartImpl() //{ */
+    3165             : 
+    3166             : std::tuple<bool, std::string> MpcTracker::gotoTrajectoryStartImpl(void) {
+    3167           4 : 
+    3168             :   std::stringstream ss;
+    3169           2 : 
+    3170           2 :   if (trajectory_set_) {
+    3171             : 
+    3172             :     toggleHover(false);
+    3173           2 : 
+    3174             :     trajectory_tracking_in_progress_ = false;
+    3175           2 : 
+    3176           2 :     {
+    3177             :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3178           2 : 
+    3179             :       setGoal((*des_x_whole_trajectory_)(0), (*des_y_whole_trajectory_)(0), (*des_z_whole_trajectory_)(0), (*des_heading_whole_trajectory_)(0),
+    3180             :               trajectory_track_heading_);
+    3181             :     }
+    3182           0 : 
+    3183           0 :     publishDiagnostics();
+    3184             : 
+    3185           0 :     ss << "flying to the start of the trajectory";
+    3186             :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3187             : 
+    3188             :     return std::tuple(true, ss.str());
+    3189             : 
+    3190             :   } else {
+    3191             : 
+    3192             :     ss << "can not fly to the start of the trajectory, the trajectory is not set";
+    3193             :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3194             : 
+    3195       23466 :     return std::tuple(false, ss.str());
+    3196             :   }
+    3197       46932 : }
+    3198       46932 : 
+    3199       46932 : //}
+    3200       46932 : 
+    3201             : // | ------------------------- support ------------------------ |
+    3202       46932 : 
+    3203             : /* //{ publishDiagnostics() */
+    3204       23466 : 
+    3205       23466 : void MpcTracker::publishDiagnostics(void) {
+    3206             : 
+    3207       23466 :   auto des_x_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_x_trajectory_);
+    3208             :   auto des_y_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_y_trajectory_);
+    3209       23466 :   auto des_z_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_z_trajectory_);
+    3210             :   auto des_heading_trajectory = mrs_lib::get_mutexed(mutex_des_trajectory_, des_heading_trajectory_);
+    3211       23466 : 
+    3212       23466 :   mrs_msgs::MpcTrackerDiagnostics diagnostics;
+    3213             : 
+    3214       23466 :   diagnostics.header.stamp    = ros::Time::now();
+    3215       23466 :   diagnostics.header.frame_id = uav_state_.header.frame_id;
+    3216       23466 : 
+    3217             :   diagnostics.active = is_active_;
+    3218       23466 : 
+    3219             :   diagnostics.uav_name = _uav_name_;
+    3220       46932 : 
+    3221             :   diagnostics.collision_avoidance_active = collision_avoidance_enabled_;
+    3222             :   diagnostics.avoiding_collision         = collision_avoidance_affecting_me_;
+    3223       46932 : 
+    3224             :   diagnostics.setpoint.position.x = des_x_trajectory(0, 0);
+    3225             :   diagnostics.setpoint.position.y = des_y_trajectory(0, 0);
+    3226       23466 :   diagnostics.setpoint.position.z = des_z_trajectory(0, 0);
+    3227             : 
+    3228       25629 :   diagnostics.setpoint.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    3229             : 
+    3230        2163 :   std::stringstream ss;
+    3231             : 
+    3232             :   {
+    3233        1429 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    3234        1429 : 
+    3235        1429 :     // fill in if other UAVs are sending their trajectories
+    3236             :     std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>::iterator u = other_uav_diagnostics_.begin();
+    3237             : 
+    3238             :     while (u != other_uav_diagnostics_.end()) {
+    3239        2163 : 
+    3240             :       if (u->second.collision_avoidance_active) {
+    3241             : 
+    3242             :         // is the other's trajectory fresh enought?
+    3243       46932 :         if ((ros::Time::now() - u->second.header.stamp).toSec() < _collision_trajectory_timeout_) {
+    3244             :           diagnostics.avoidance_active_uavs.push_back(u->first);
+    3245       23466 :           ss << u->first.c_str() << ", ";
+    3246        1429 :         }
+    3247       47914 :       }
+    3248       25877 : 
+    3249       18021 :       u++;
+    3250             :     }
+    3251             :   }
+    3252       23466 : 
+    3253             :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3254       46932 : 
+    3255             :   if (ss.str().length() > 0) {
+    3256       23466 :     ROS_DEBUG_STREAM_THROTTLE(5.0, "[MpcTracker]: getting avoidance trajectories: " << ss.str());
+    3257             :   } else if (collision_avoidance_enabled_ &&
+    3258       22037 :       (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    3259             :     ROS_DEBUG_THROTTLE(10.0, "[MpcTracker]: missing avoidance trajectories!");
+    3260             :   }
+    3261             : 
+    3262        1429 :   pub_diagnostics_.publish(diagnostics);
+    3263             : 
+    3264             :   std_msgs::String string_msg;
+    3265       23466 : 
+    3266             :   if (diagnostics.avoidance_active_uavs.empty()) {
+    3267       24895 : 
+    3268        1429 :     string_msg.data = "-id col_avoid I see: NOTHING";
+    3269        1429 : 
+    3270             :   } else {
+    3271           0 : 
+    3272             :     string_msg.data = "-id col_avoid I see: ";
+    3273             :   }
+    3274             : 
+    3275             :   if (diagnostics.avoidance_active_uavs.size() <= 3) {
+    3276             : 
+    3277           0 :     for (size_t i = 0; i < diagnostics.avoidance_active_uavs.size(); i++) {
+    3278           0 :       if (i == 0) {
+    3279             :         string_msg.data += diagnostics.avoidance_active_uavs.at(i);
+    3280           0 :       } else {
+    3281             :         string_msg.data += ", " + diagnostics.avoidance_active_uavs.at(i);
+    3282             :       }
+    3283       23466 :     }
+    3284       23466 : 
+    3285             :   } else {
+    3286             : 
+    3287             :     std::stringstream ss;
+    3288             :     ss << diagnostics.avoidance_active_uavs.size();
+    3289             : 
+    3290           0 :     string_msg.data += ss.str() + " UAVs";
+    3291             :   }
+    3292           0 : 
+    3293             :   pub_status_string_.publish(string_msg);
+    3294           0 : }
+    3295           0 : 
+    3296           0 : //}
+    3297           0 : 
+    3298           0 : /* debugPrintState() //{ */
+    3299             : 
+    3300             : void MpcTracker::debugPrintState(const double throttle) {
+    3301             : 
+    3302             :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3303             : 
+    3304           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: pos [%.2f, %.2f, %.2f, %.2f]", mpc_x(0), mpc_x(4), mpc_x(8), mpc_x_heading(0));
+    3305             :   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));
+    3306           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: acc [%.2f, %.2f, %.2f, %.2f]", mpc_x(2), mpc_x(6), mpc_x(10), mpc_x_heading(2));
+    3307           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC internal state: jerk [%.2f, %.2f, %.2f, %.2f]", mpc_x(3), mpc_x(7), mpc_x(11), mpc_x_heading(3));
+    3308             : }
+    3309           0 : 
+    3310           0 : //}
+    3311             : 
+    3312           0 : /* debugPrintMPCu() //{ */
+    3313             : 
+    3314             : void MpcTracker::debugPrintMPCResult(const double throttle) {
+    3315             : 
+    3316             :   auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    3317             :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    3318        9232 : 
+    3319             :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC result: [%.2f, %.2f, %.2f, %.2f]", mpc_u(0), mpc_u(1), mpc_u(2), mpc_u_heading);
+    3320        9232 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: snap constraint: hor: %.2f, ver asc: %.2f, vert desc: %.2f, heading: %.2f]", constraints.horizontal_snap,
+    3321        9232 :                      constraints.vertical_ascending_snap, constraints.vertical_descending_snap, constraints.heading_snap);
+    3322             : }
+    3323        9232 : 
+    3324             : //}
+    3325             : 
+    3326             : /* getCurrentTrajectoryIdx() //{ */
+    3327             : 
+    3328             : int MpcTracker::getCurrentTrajectoryIdx() {
+    3329             : 
+    3330       15588 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3331             :   auto trajectory_dt   = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_dt_);
+    3332       15588 : 
+    3333       15588 :   return floor(trajectory_current_time / trajectory_dt);
+    3334             : }
+    3335       15588 : 
+    3336             : //}
+    3337       15588 : 
+    3338             : /* increaseCurrentTrajectoryTime() //{ */
+    3339             : 
+    3340       15588 : void MpcTracker::increaseCurrentTrajectoryTime(const double dt) {
+    3341             : 
+    3342           5 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3343             :   auto [trajectory_size, trajectory_dt] = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_, trajectory_dt_);
+    3344             : 
+    3345           0 :   trajectory_current_time += dt;
+    3346             : 
+    3347           0 :   const double trajectory_duration = trajectory_size * trajectory_dt;
+    3348             : 
+    3349             :   // if the tracking idx hits the end of the trajectory
+    3350             :   if (trajectory_current_time >= trajectory_duration) {
+    3351           5 : 
+    3352             :     if (trajectory_tracking_loop_) {
+    3353           5 : 
+    3354             :       // reset the idx
+    3355             :       trajectory_current_time -= trajectory_duration;
+    3356             : 
+    3357       15588 :       ROS_INFO("[MpcTracker]: trajectory looped");
+    3358       15583 : 
+    3359             :     } else {
+    3360       15588 : 
+    3361             :       trajectory_tracking_in_progress_ = false;
+    3362             : 
+    3363             :       ROS_INFO("[MpcTracker]: done tracking trajectory, current time in trajectory=%f, trajectory_duration=%f", trajectory_current_time, trajectory_duration);
+    3364             :     }
+    3365             :   }
+    3366             : 
+    3367             :   if (trajectory_tracking_in_progress_) {
+    3368             :     mrs_lib::set_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time, trajectory_current_time_);
+    3369             :   }
+    3370             : }
+    3371       22147 : 
+    3372             : //}
+    3373       22147 : 
+    3374           0 : // --------------------------------------------------------------
+    3375             : // |                           timers                           |
+    3376       66441 : // --------------------------------------------------------------
+    3377       66441 : 
+    3378             : /* //{ timerDiagnostics() */
+    3379       22147 : 
+    3380             : // published diagnostics in reguar intervals
+    3381             : void MpcTracker::timerDiagnostics(const ros::TimerEvent& event) {
+    3382             : 
+    3383             :   if (!is_initialized_)
+    3384             :     return;
+    3385             : 
+    3386       86608 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.1, event);
+    3387             :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3388       86608 : 
+    3389           0 :   publishDiagnostics();
+    3390           0 : }
+    3391             : 
+    3392             : //}
+    3393       86608 : 
+    3394             : /* //{ timerMPC() */
+    3395       86608 : 
+    3396             : void MpcTracker::timerMPC(const ros::TimerEvent& event) {
+    3397       86608 : 
+    3398             :   if (odometry_reset_in_progress_) {
+    3399       86608 :     ROS_ERROR("[MpcTracker]: mpc iteration tried run while reseting odometry");
+    3400           0 :     return;
+    3401             :   }
+    3402             : 
+    3403       86608 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3404           0 : 
+    3405             :   mrs_lib::AtomicScopeFlag unset_running(mpc_timer_running_);
+    3406             : 
+    3407      259824 :   bool started_with_invalid = mpc_result_invalid_;
+    3408      259824 : 
+    3409             :   if (!is_active_) {
+    3410       86608 :     return;
+    3411       86608 :   }
+    3412       86608 : 
+    3413             :   if (!is_initialized_) {
+    3414             :     return;
+    3415             :   }
+    3416       86608 : 
+    3417             :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerMPC", 1.0 / dt1, 0.01, event);
+    3418       31178 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerMPC", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3419       31178 : 
+    3420             :   ros::Time     begin = ros::Time::now();
+    3421             :   ros::Time     end;
+    3422             :   ros::Duration interval;
+    3423       15589 :   int           trajectory_id;
+    3424             : 
+    3425       15589 :   // if we are tracking trajectory, copy the setpoint
+    3426       15589 :   if (trajectory_tracking_in_progress_) {
+    3427       15589 : 
+    3428       15589 :     MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    3429             :     VectorXd des_x_whole_trajectory, des_y_whole_trajectory, des_z_whole_trajectory, des_heading_whole_trajectory;
+    3430       15589 :     double   trajectory_dt;
+    3431       15589 :     int      trajectory_size;
+    3432       15589 :     {
+    3433       15589 :       std::scoped_lock lock(mutex_des_trajectory_, mutex_des_whole_trajectory_);
+    3434             : 
+    3435       15589 :       des_x_trajectory       = des_x_trajectory_;
+    3436       15589 :       des_y_trajectory       = des_y_trajectory_;
+    3437             :       des_z_trajectory       = des_z_trajectory_;
+    3438       15589 :       des_heading_trajectory = des_heading_trajectory_;
+    3439             : 
+    3440             :       des_x_whole_trajectory       = *des_x_whole_trajectory_;
+    3441             :       des_y_whole_trajectory       = *des_y_whole_trajectory_;
+    3442             :       des_z_whole_trajectory       = *des_z_whole_trajectory_;
+    3443       15589 :       des_heading_whole_trajectory = *des_heading_whole_trajectory_;
+    3444             : 
+    3445       15589 :       trajectory_size = trajectory_size_;
+    3446       15588 :       trajectory_dt   = trajectory_dt_;
+    3447             : 
+    3448           1 :       trajectory_id = des_whole_trajectory_id_;
+    3449             :     }
+    3450             : 
+    3451       15589 :     /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    3452             : 
+    3453      639149 :     const double dt_from_last_update = (event.current_real - event.last_real).toSec();
+    3454             : 
+    3455      623560 :     if (dt_from_last_update > 0.0 && dt_from_last_update < 1.0) {
+    3456             :       increaseCurrentTrajectoryTime(dt_from_last_update);
+    3457      623560 :     } else {
+    3458      623560 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: timerMpc(): dt from the last update is not normal, not iterating trajectory, dt=%.4f", dt_from_last_update);
+    3459             :     }
+    3460      623560 : 
+    3461             :     auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3462      623560 : 
+    3463             :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3464           0 : 
+    3465           0 :       double first_time = trajectory_current_time + double(i) * _dt2_;
+    3466             : 
+    3467             :       int first_idx  = int(floor(first_time / trajectory_dt));
+    3468           0 :       int second_idx = first_idx + 1;
+    3469           0 : 
+    3470             :       double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    3471             : 
+    3472             :       if (trajectory_tracking_loop_) {
+    3473             : 
+    3474      623560 :         if (second_idx >= trajectory_size) {
+    3475       93432 :           second_idx = second_idx % trajectory_size;
+    3476             :         }
+    3477             : 
+    3478      623560 :         if (first_idx >= trajectory_size) {
+    3479       86084 :           first_idx = first_idx % trajectory_size;
+    3480             :         }
+    3481             : 
+    3482             :       } else {
+    3483      623560 : 
+    3484           0 :         if (second_idx >= trajectory_size) {
+    3485           0 :           second_idx = trajectory_size - 1;
+    3486             :         }
+    3487             : 
+    3488      623560 :         if (first_idx >= trajectory_size) {
+    3489      623560 :           first_idx = trajectory_size - 1;
+    3490      623560 :         }
+    3491             :       }
+    3492      623560 : 
+    3493             :       if (first_idx < 0 || first_idx > (trajectory_size-1) || second_idx < 0 || second_idx > (trajectory_size-1)) {
+    3494             :         ROS_ERROR_THROTTLE(0.1, "[MpcTracker]: trying to index out range when interpolating the trajectory! first_idx=%d, second_idx=%d, trajectory_size=%d", first_idx, second_idx, trajectory_size);
+    3495             :         continue;
+    3496       31178 :       }
+    3497             : 
+    3498       15589 :       des_x_trajectory(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    3499       15589 :       des_y_trajectory(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    3500       15589 :       des_z_trajectory(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    3501       15589 : 
+    3502             :       des_heading_trajectory(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    3503             :     }
+    3504             : 
+    3505             :     {
+    3506             :       std::scoped_lock lock(mutex_des_trajectory_);
+    3507             : 
+    3508       71019 :       des_x_trajectory_       = des_x_trajectory;
+    3509             :       des_y_trajectory_       = des_y_trajectory;
+    3510       71019 :       des_z_trajectory_       = des_z_trajectory;
+    3511             :       des_heading_trajectory_ = des_heading_trajectory;
+    3512             :     }
+    3513       86608 : 
+    3514             :     //}
+    3515       86608 : 
+    3516             :   } else {
+    3517       86608 : 
+    3518       86608 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3519             : 
+    3520             :     trajectory_id = des_whole_trajectory_id_;
+    3521             :   }
+    3522       86608 : 
+    3523             :   manageConstraints();
+    3524       86608 : 
+    3525           0 :   calculateMPC();
+    3526             : 
+    3527             :   end      = ros::Time::now();
+    3528             :   interval = end - begin;
+    3529             : 
+    3530             :   // | ------------------ calculate the MPC RTF ----------------- |
+    3531      173216 : 
+    3532       86608 :   mpc_rtf_ = 0.99 * mpc_rtf_ + 0.01 * (interval.toSec()/dt1);
+    3533       86608 : 
+    3534             :   if (mpc_rtf_ >= 1.0) {
+    3535             :     ROS_WARN_THROTTLE(5.0, "[MpcTracker] MPC Real Time Factor (%.3f) is slow", mpc_rtf_);
+    3536      173216 :   }
+    3537             : 
+    3538     3550932 :   /* publish predicted future //{ */
+    3539             : 
+    3540     3464320 :   {
+    3541             :     geometry_msgs::PoseArray debug_trajectory_out;
+    3542     3464320 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    3543     3464320 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    3544     3464320 : 
+    3545             :     {
+    3546             :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3547     3464320 : 
+    3548           0 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3549           0 : 
+    3550             :         geometry_msgs::Pose newPose;
+    3551             : 
+    3552     3464320 :         newPose.position.x = predicted_trajectory_(i * MPC_N_STATES);
+    3553             :         newPose.position.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3554             :         newPose.position.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3555             : 
+    3556       86608 :         try {
+    3557             :           newPose.orientation = mrs_lib::AttitudeConverter(0, 0, predicted_heading_trajectory_(i * MPC_N_STATES));
+    3558             :         } catch (...) {
+    3559             :           ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: failed to fill orientation into debug print trajectory");
+    3560             :         }
+    3561             : 
+    3562             :         debug_trajectory_out.poses.push_back(newPose);
+    3563             :       }
+    3564      173216 :     }
+    3565       86608 : 
+    3566       86608 :     ph_predicted_trajectory_debugging_.publish(debug_trajectory_out);
+    3567             :   }
+    3568       86608 : 
+    3569             :   //}
+    3570       86608 : 
+    3571             :   /* publish full state prediction //{ */
+    3572             : 
+    3573      173216 :   {
+    3574             :     mrs_msgs::MpcPredictionFullState prediction_fs_out;
+    3575     3550932 :     prediction_fs_out.header.stamp    = ros::Time::now();
+    3576             :     prediction_fs_out.header.frame_id = uav_state_.header.frame_id;
+    3577     3464320 : 
+    3578       86608 :     ros::Time stamp = prediction_fs_out.header.stamp;
+    3579             : 
+    3580     3377708 :     prediction_fs_out.input_id = trajectory_id;
+    3581             : 
+    3582             :     {
+    3583     3464320 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3584             : 
+    3585             :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3586     3464320 : 
+    3587             :         if (i == 0) {
+    3588     3464320 :           stamp += ros::Duration(0.01);
+    3589     3464320 :         } else {
+    3590     3464320 :           stamp += ros::Duration(0.2);
+    3591             :         }
+    3592     3464320 : 
+    3593             :         prediction_fs_out.stamps.push_back(stamp);
+    3594             : 
+    3595             :         {  // position
+    3596     3464320 :           geometry_msgs::Point point;
+    3597             : 
+    3598     3464320 :           point.x = predicted_trajectory_(i * MPC_N_STATES);
+    3599     3464320 :           point.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3600     3464320 :           point.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3601             : 
+    3602     3464320 :           prediction_fs_out.position.push_back(point);
+    3603             :         }
+    3604             : 
+    3605             :         {  // velocity
+    3606     3464320 :           geometry_msgs::Vector3 vector;
+    3607             : 
+    3608     3464320 :           vector.x = predicted_trajectory_(i * MPC_N_STATES + 1);
+    3609     3464320 :           vector.y = predicted_trajectory_(i * MPC_N_STATES + 5);
+    3610     3464320 :           vector.z = predicted_trajectory_(i * MPC_N_STATES + 9);
+    3611             : 
+    3612     3464320 :           prediction_fs_out.velocity.push_back(vector);
+    3613             :         }
+    3614             : 
+    3615             :         {  // acceleration
+    3616     3464320 :           geometry_msgs::Vector3 vector3;
+    3617             : 
+    3618     3464320 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 2);
+    3619     3464320 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 6);
+    3620     3464320 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 10);
+    3621             : 
+    3622     3464320 :           prediction_fs_out.acceleration.push_back(vector3);
+    3623             :         }
+    3624             : 
+    3625             :         {  // jerk
+    3626             :           geometry_msgs::Vector3 vector3;
+    3627             : 
+    3628     3464320 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 3);
+    3629     3464320 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 7);
+    3630     3464320 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 11);
+    3631     3464320 : 
+    3632             :           prediction_fs_out.jerk.push_back(vector3);
+    3633             :         }
+    3634             : 
+    3635             :         {
+    3636             :           // heading
+    3637      173216 : 
+    3638       86608 :           prediction_fs_out.heading.push_back(predicted_heading_trajectory_(i * MPC_N_STATES));
+    3639             :           prediction_fs_out.heading_rate.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 1));
+    3640             :           prediction_fs_out.heading_acceleration.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 2));
+    3641             :           prediction_fs_out.heading_jerk.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 3));
+    3642             :         }
+    3643             :       }
+    3644       86608 :     }
+    3645             : 
+    3646       86608 :     {
+    3647             :       std::scoped_lock lock(mutex_prediction_full_state_);
+    3648           5 :       prediction_full_state_ = prediction_fs_out;
+    3649             :     }
+    3650           5 :   }
+    3651             : 
+    3652             :   //}
+    3653             : 
+    3654             :   mpc_computed_ = true;
+    3655             : 
+    3656             :   if (started_with_invalid) {
+    3657             : 
+    3658         739 :     mpc_result_invalid_ = false;
+    3659             : 
+    3660         739 :     ROS_INFO("[MpcTracker]: calculated the first MPC result after invalidation");
+    3661           3 :   }
+    3662             : }
+    3663             : 
+    3664         739 : //}
+    3665           0 : 
+    3666             : /* timerVelocityTracking() //{ */
+    3667             : 
+    3668        1478 : void MpcTracker::timerVelocityTracking(const ros::TimerEvent& event) {
+    3669             : 
+    3670        1478 :   if (!is_initialized_) {
+    3671             :     return;
+    3672             :   }
+    3673         739 : 
+    3674             :   if (!velocity_tracking_active_) {
+    3675           3 :     return;
+    3676           3 :   }
+    3677             : 
+    3678           3 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerVelocityTracking", int(30.0), 0.01, event);
+    3679             :   mrs_lib::ScopeTimer timer =
+    3680           3 :       mrs_lib::ScopeTimer("MpcTracker::timerVelocityTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3681             : 
+    3682           3 :   // stop the timer when timeout
+    3683             :   if ((ros::Time::now() - velocity_reference_time_).toSec() > 0.5) {
+    3684             : 
+    3685        1472 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: velocity reference timeouted, hovering");
+    3686         736 :     timer_velocity_tracking_.stop();
+    3687             : 
+    3688        1472 :     toggleHover(true);
+    3689             : 
+    3690         736 :     velocity_tracking_active_ = false;
+    3691         736 : 
+    3692         736 :     return;
+    3693         736 :   }
+    3694         736 : 
+    3695             :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3696         736 :   auto velocity_reference     = mrs_lib::get_mutexed(mutex_velocity_reference_, velocity_reference_);
+    3697         736 : 
+    3698         736 :   mrs_msgs::TrajectoryReference trajectory;
+    3699         736 : 
+    3700             :   trajectory.fly_now         = true;
+    3701       37536 :   trajectory.use_heading     = true;
+    3702             :   trajectory.dt              = 0.2;
+    3703       36800 :   trajectory.header.stamp    = ros::Time::now();
+    3704       36800 :   trajectory.header.frame_id = "";
+    3705       36800 : 
+    3706       36800 :   double x       = mpc_x(0, 0);
+    3707       36800 :   double y       = mpc_x(4, 0);
+    3708             :   double z       = mpc_x(8, 0);
+    3709       36800 :   double heading = mpc_x_heading(0, 0);
+    3710             : 
+    3711       36800 :   for (int i = 0; i < 50; i++) {
+    3712       36800 : 
+    3713       36800 :     mrs_msgs::Reference reference;
+    3714             :     reference.position.x = x;
+    3715       36800 :     reference.position.y = y;
+    3716           0 :     reference.position.z = z;
+    3717             :     reference.heading    = heading;
+    3718             : 
+    3719       36800 :     trajectory.points.push_back(reference);
+    3720       33400 : 
+    3721        3400 :     x += velocity_reference.velocity.x * trajectory.dt;
+    3722           0 :     y += velocity_reference.velocity.y * trajectory.dt;
+    3723             :     z += velocity_reference.velocity.z * trajectory.dt;
+    3724             : 
+    3725             :     if (velocity_reference.use_altitude) {
+    3726        1472 :       z = velocity_reference.altitude;
+    3727             :     }
+    3728             : 
+    3729             :     if (velocity_reference.use_heading_rate) {
+    3730             :       heading += velocity_reference.heading_rate * trajectory.dt;
+    3731             :     } else if (velocity_reference.use_heading) {
+    3732             :       heading = velocity_reference.heading;
+    3733        4395 :     }
+    3734             :   }
+    3735        4395 : 
+    3736        2106 :   auto [success, message, modified] = loadTrajectory(trajectory);
+    3737             : }
+    3738             : 
+    3739        2325 : //}
+    3740           0 : 
+    3741             : /* //{ timerAvoidanceTrajectory() */
+    3742             : 
+    3743        2325 : void MpcTracker::timerAvoidanceTrajectory(const ros::TimerEvent& event) {
+    3744           0 : 
+    3745             :   if (!is_active_) {
+    3746             :     return;
+    3747             :   }
+    3748        2325 : 
+    3749        2325 :   if (!is_initialized_) {
+    3750             :     return;
+    3751        2325 :   }
+    3752        2325 : 
+    3753             :   if (!sh_estimation_diag_.hasMsg()) {
+    3754        2325 :     return;
+    3755          36 :   } else {
+    3756             :     // we won't try to transform and publish the avoidance prediction if we cannot transform it
+    3757             : 
+    3758             :     auto                     estimation_diag      = sh_estimation_diag_.getMsg();
+    3759        4578 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    3760             : 
+    3761        4578 :     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();
+    3762             :     bool got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    3763        2289 : 
+    3764        2289 :     if (!got_gps_est && !got_rtk_est) {
+    3765             :       return;
+    3766        2289 :     }
+    3767             :   }
+    3768        2289 : 
+    3769             :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerAvoidanceTrajectory", _avoidance_trajectory_rate_, 0.1, event);
+    3770             :   mrs_lib::ScopeTimer timer =
+    3771        2289 :       mrs_lib::ScopeTimer("MpcTracker::timerAvoidanceTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3772        2289 : 
+    3773        2289 :   auto uav_state            = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3774        2289 :   auto predicted_trajectory = mrs_lib::get_mutexed(mutex_predicted_trajectory_, predicted_trajectory_);
+    3775        2289 : 
+    3776        2289 :   if (future_was_predicted_) {
+    3777        2289 : 
+    3778        2289 :     mrs_msgs::FutureTrajectory avoidance_trajectory;
+    3779        2289 : 
+    3780             :     // fill last trajectory with initial data
+    3781        4578 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3782             :     avoidance_trajectory.uav_name            = _uav_name_;
+    3783        2289 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3784             :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk");
+    3785           0 :     avoidance_trajectory.points.clear();
+    3786           0 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3787           0 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3788           0 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3789             :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_;
+    3790             : 
+    3791             :     auto res = common_handlers_->transformer->getTransform(uav_state.header.frame_id, "utm_origin", ros::Time::now());
+    3792        4578 : 
+    3793             :     if (!res) {
+    3794       93849 : 
+    3795             :       std::string message = "[MpcTracker]: can not transform predicted future to utm_origin";
+    3796             :       ROS_WARN_STREAM_ONCE(message);
+    3797      183120 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3798             :       return;
+    3799       91560 : 
+    3800       91560 :     } else {
+    3801             : 
+    3802       91560 :       geometry_msgs::TransformStamped tf = res.value();
+    3803       91560 : 
+    3804       91560 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3805             : 
+    3806       91560 :         // original point
+    3807             :         geometry_msgs::PoseStamped original_point;
+    3808      183120 : 
+    3809             :         original_point.header.stamp    = ros::Time::now();
+    3810       91560 :         original_point.header.frame_id = uav_state.header.frame_id;
+    3811             : 
+    3812       91560 :         original_point.pose.position.x = predicted_trajectory(i * MPC_N_STATES);
+    3813             :         original_point.pose.position.y = predicted_trajectory(i * MPC_N_STATES + 4);
+    3814       91560 :         original_point.pose.position.z = predicted_trajectory(i * MPC_N_STATES + 8);
+    3815       91560 : 
+    3816       91560 :         original_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    3817             : 
+    3818       91560 :         auto res = common_handlers_->transformer->transform(original_point, tf);
+    3819             : 
+    3820             :         if (res) {
+    3821             : 
+    3822           0 :           mrs_msgs::FuturePoint new_point;
+    3823           0 : 
+    3824           0 :           new_point.x = res.value().pose.position.x;
+    3825             :           new_point.y = res.value().pose.position.y;
+    3826             :           new_point.z = res.value().pose.position.z;
+    3827             : 
+    3828             :           avoidance_trajectory.points.push_back(new_point);
+    3829        2289 : 
+    3830             :         } else {
+    3831             : 
+    3832             :           std::string message = "[MpcTracker]: can not transform a point of a future trajectory";
+    3833             :           ROS_WARN_STREAM_ONCE(message);
+    3834             :           ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3835             :         }
+    3836             :       }
+    3837         171 :     }
+    3838             : 
+    3839         342 :     ph_avoidance_trajectory_.publish(avoidance_trajectory);
+    3840             :   }
+    3841         513 : }
+    3842         513 : 
+    3843             : //}
+    3844         171 : 
+    3845             : /* timerHover() //{ */
+    3846         171 : 
+    3847             : void MpcTracker::timerHover(const ros::TimerEvent& event) {
+    3848          78 : 
+    3849             :   MatrixXd mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    3850          78 : 
+    3851             :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerHover", 10, 0.01, event);
+    3852         171 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerHover", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3853             : 
+    3854             :   setRelativeGoal(0, 0, 0, 0, false);
+    3855             : 
+    3856             :   if (fabs(mpc_x(1, 0)) < 0.1 && fabs(mpc_x(5, 0)) < 0.1 && fabs(mpc_x(9, 0)) < 0.1) {
+    3857             : 
+    3858             :     toggleHover(false);
+    3859             : 
+    3860             :     ROS_INFO("[MpcTracker]: timerHover: speed is low, stopping hover timer");
+    3861         107 :   }
+    3862             : }
+    3863             : 
+    3864             : //}
+    3865             : 
+    3866             : }  // namespace mpc_tracker
+    3867             : 
+    3868             : }  // namespace mrs_uav_trackers
+    3869             : 
+    3870             : #include <pluginlib/class_list_macros.h>
+    3871             : 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..dde0b58097 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html @@ -0,0 +1,988 @@ + + + + + + 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 + + +
+ 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..915f370e224b5818f3060c133d548194eeac73f4 GIT binary patch literal 13661 zcmV-jHKNLiP)uaYJ1rZSeAJ66ae6BzL zd)(E2@HwQ<7Rn6R1T}XZ+o}Z|9Rk59^viZV)^UkZ?3-rjWss~WJc>sz=vXRdFZ>PH}Yy=_?sj7!_BPy-sfUYDja4OHLQ zcR|bmu7L4=>473%1H8Rw&~*d%gsvCAYTO9!!5B!nW*aD>pC48VsGY>T%_M|N)le*c zB>^hBj{U*z8B391l~;`bIKX^{!u3H1x;*pLAjtAt3b?{S0NqVHgrCr*jj_rZc(mkM7qE002J(aujJh#)b=~qzrC+N~*}AUdthf!f z3HS^<@o2vnMr08#gnh;SuEB`sEl(c{$oySDS!~zb3`t%0&t}o7G?&ci2ImsbSlzF9 z`x0CyRlvo2``uR-@cLAR zF#=XFlBK|Jn@MDDlioao*XenE!ZFqXGtm;ppC5D>1E7RaPs|dx_BEc<0?P7q2JGmD zvw0?91r{u>;aLtC6MG!5mv}M`(}TOd*@p~I;{MCZTD(ZEpOrEEK|l)2u3g8orIhd| z2iPy+E81piTU_ZvScVQ6VD=h-0uPJrJz;8*lsHz9G(ybkg|dEB{29*zM@FX@j&Xq$ zEQQiZfN1&4Mo$l5>{w5hT6kz-F~(#G=x`5Mol&JUQ6N4(D@0L15g0POg;Yrl zY2pS-KhYf=EL{VVFy?W<`WC7fV>}0sA6EQ^cH{L`;R7H}{$U>g$Hxx>K8>7ne{ri_ zn1<5gEG4cDc*pQO#sxTo(oVU=Px^$yDd+n0u+Ft3i@F)^??Xtx*jB(?RtI2^N|170dn_Yw^F?#;-mj( z<+>i~7k3@BH|;Y5GE|S*h19Or0^Z^8F`cjD%pfSD$3^3!+YEzuaX!E)4#a=Ac&TKM zDTB6OJh!dM8x=P_3Ro;cMoJcw4sW&S7nQn9LqveJF3oOwhCMt2Mn`c<%hGkb#mEAl zJ+M0+dmea;(Ts6Hoi9myLQnR>7)U(uE$kURoRt$-0ETb)ZE2)yuH{}V7@>g`L#e=I zb)MqzVC8saG!K$1E@6Sx4hBGyuBE=I4qw0@_Kg5CJROd}UNXRNL>j{*EEq_3@*G43 zu<9^!IHa2j{?B-fYuR<@1_l_OEvm8O9zwwJB>_H)#Sb}cCudUNg+_Hep|80P_&`*T zQS2}xs?(|&r8<36y&8gl@su?{L9HaQ48~DsDs^aJ4h?wXvR?zYRST%aSZW&{aFjjq zVinC&2YZ&WA=NR~?%9aqXHxJEgXh6MDuQv24o?$S1tfnl90y63H3w8 znoq-)uA>IHDPtOeWd|`b9Y&0gmg>pIFwj3q@d0rKfJ%(&78Xs|tMe_*8U|s; z2>6IaoqXOqGZasrdJ9H=XgdY``uYa#b^ZVK{r>r%Z(p%_z@Oj0zv}Jr)q;qC<|%yU zO%r)4hNiIE41k8Y_9`NLJt(RKXuw!+d&d~1hQ@0E)OWpOBnPk13&DNmAAW@Q^?qNU zd{}Tdr@)a8zhzfwF?_eo%!DbdwT#pNsPEb_yJ-U$0QFsekm{=?F%yY00BXDTH{$VD zuTSFwlj(jXb$og$IA;HF`r>+ZW*p$A1w8#c-Y|uk>Z7_KFrXUaDZ_qrLs9|mpWQSy zfD_it{d~NAjEA~wwE{SP2_OFhya!M}*U~BPYWYC*D~?i>J)X3~|96?H`}KW?YA(+) zq|F0#u=O$4{7bp*Hma|0cIxap7(j=+6e0v{>zZPdsMcdl?5CX5kVIgk7K2LMm# zwQB$}vbK*M#OVwT?uAs)NCRDqfQPN?R20xVT4WasURpb$^-L0l8ScOAUh5pllSY*k zp$M|aN=%GV7U?PV#LnrV;Kn)1t$}JH!(4BfGe0EXaX5pqz%?HkR*z%~`;-*iHFwOx zqZ1>i!(~9t(Oz*^usJ?zch^nPuj^XZv!QF4R^1&;go)wSAkRlFG5MwwR1g@sCff>G z;ME_ut~G#%8e;`e%N|C)4`v7BZCp-E*)MXMlLESSB3$$u#zDte4ley75>)I$ytMKOVC%Vs+9E?}z!r&=) z3qWDjvfHpI6lV4yW9~V|9_dK9=YP7#cte06F}Xxh6URuXIsiuZdkNzXyRHGA)X?O_ z3<1WWRRB8~zJN%^L53go3!hHUM=F#Be|Q?CTGfMhGM1)fQ$xv;pHvOLOtM7*LM-{%gfmnssN5W-c)i5+scVq+u#c3 zb;MFEMbIul)iU%FuCVvSt~o$m*RG0@CXY_p^G9`0MJVW%uTtaIFdi6i#=UFqxjIHQ zpsqbi53&npIMR=&6{A+lERCAi&lCsIcU8=9*CKlgd8h%Z7_I_TGW?$;rjRj0_ z7s(-W4*}Ai`IHIUMfV#1dxC;{;pza8?JFa$q2xn$xwztq7!VW|QSov2p?&9ZeREQ7 z_S`G90PPt4G~a>o!#QUkN&MQH9JFIpUl-stBS(=;(G-b^6}kLXw^9>pKZrV9%bu+{ z%Y7KZC!(@f?+O4WVH_Ic(0 zD_+X)%TcN2qvcwN>`VO^|8h}bR8u&QP#=S4{@^O|8xtw(ozQBv1?;0f9 zz(Z+q%He9dutDIagO40U2Eq_+j>;va<38miW9M{1ZhbZ-*X$$pk7no?PUn|+cWqv- zD-);Qm$0mP1*Zfn&~jv9(h^Zz<@DHP4tWxlem*h3 zF-p1}*G!~Z1XP{w!__&cCKM?r-Wp1Eb&Ms|#~E&%50^I{s}^BeDr3~pG9$g{;U8s6 z7t#5#rZt=SS26S?JdH4fxD?vNOuj?fW%20?v&}WXP|BYb<5rB(iO&&#+`T0A1nxGZ z1B)Zhiy^@wNNmMRdGi><(XnC&Y=tA2=I?N0ydN+yg}ZfT08jRMx`#b$qs~W@n3RSv z74ReMS({EN()A{}&gTP)UV9VOI>*@t`aY@&ba>|yLn`zek}?o51;xgLp?Dl8fHCPK zJ6_#CKVK)r?PW1*GQ2s>k&pKS>RO-lToNu-IV#N=VWiWg$GItl9G}lg&j#+L>lrx{ z?4G-mftZ~9V3{20qr-|(Naw%@u~N)V{lfb)W40Xvg|9={tVXb9CxVzjZY zEuo0@Qzl8c^2A={3INmz52+}bG)i>u#Vexs#H7=xRJi(h?mt+N_+%^Yg=X{4fZ}uV z@d75s))@{8u+(@@HH8S95^dg+lR57ST4g_>e21r{C2Bp#y;z1|ji+|(ye5{tk9(7& zkgb40hqeH(pKE|hs{bR#Obq9QVTlm{dx&ujDXM!gZro9m-wSnscVl#)wgA(o%C6ZX z+Juc=-GnfSSK1pxpoF=kg930`NrzSqmIqrpjMf8(mr|z^jd8v=al!X{+W_4d}@k!>A;bTVV|_ z%w!0=Aa|nOxd7+MhtAm7O!(7`&Bv;wh?N}y9-NOmKoVa{1awgtId<Tv2#*e_ZftuZ;X`Bsj0dS-KzoBjlsB*$Ke~N1MQlL+>h+h z8c2GEpV%4}0q@TSRJ}7|!w4Q|uf_owW_m5Bn)#q(LuwVN)k{x|=WQgPcdf#pgJi%x zY9(NR6k9m&wO_F~b{LP%zafm&mV1JDGEtEe_1Jd{X@)ZhWB9)gen?@T7=%trdz zZ&3hQ2v51_jUJ;)0ob;WV&9PDE(2XZH6#61UAtYW-2O;9qSDT^+Tn`%2H_@qJOcTU zcyIWG504m7rAtHb(d9=_k{&_$|DGbxBVYiPzhOdRn2U>(3;N<!36}^#H^43J>dQXNr;dvli!5no!mA=rp z`nn)wXTH+S!yDaH#-KxUO5PQ0Qq3j>DfEv<%G(db(o~`k%jrX*P68ieAa1?=sa$&; zqv^g)Yi4QPyv#UM{6l<vaH$515D_}{C?3TLU~ooEdkb^PCiUm zm7r^062s3!fNTOI19&E{1qD=Rr|PLz0diCQ91ZW_kB!7*812W_RQJ9OSXPxy2f1!Q zF;TTR`0UI)2Zql`%1!kvYG4-WNzy8DDb9>1vN4HKDAvLe9ezetG>m|2%T1tUxIxVP z3H{(E7z5fEelXX)7*J#n!vlAsTw{FC8p@S~GiH}iXaNCxYA1Hf%y{;&8KiiOI>5lk zAC5GujNH9kA$yL*CZ3OJt?d<0DF$Q%BqHGL2cHKodK}dzFPEksfWdiXAO`MTF>#__ z1fb)+0ohp1u}wGv5M@35Bs z-66HEhO%Ve_;%{QCgGWg;R?_?3BMc~YB4iNPkJwfhA4E_6SK=>Gqq6*sAc%SERVIa z6H1Iz?leU)>y+gG#H>xNP=#p>f1oo*2;gUSp3*B$EUn*~FYQ5TmaMq{Ya| zkNX`u{|JB%SPOWx0mCFyz`KKN^z2Cjv9}!ON{m1xnqePiAxqA3cC~h41RAtN*yYeT z`FkcKA0?A33R04u-(_;OuM3nI`D<&!u|8k(ZTAa|j4^v=ci?WN(rBOHR&>C)5r@Mn zrWkSRnNt&k&kA=$b64UmknsLtBMxzH!ag%@Akkz=b=N}nE(2)Cn6`XAL~CZD$m)c5 zvM1gGf^P;fvaA*Xh3iy9Lo!{7$(@^M1Jyl+)0);}|QU zaImWf492PbNrxSNTS^}CAjQ>yn*mwCs1)(kK`Q{t4Csldl~*?~{9f}~9n9H$dd}{= z!i56xVg{Q;zbjr^UY5nhvzxA;33iXq(o$fY7|O8`ZBD5Mr;lefn0IStYTNmEccvY# z_5F%5;)UGX9x#2y2OZxjPCPz5=yQWykap@dok0YQmaf13n}+EL(NsAYP~|l)lh$8o zOT*0Un)E`uZRR4~;>z1|c121UD{Zp|#@qzwhjEOmJk9C8<`lhmP(VG#%cDt3PX<20 zne8^OqhEys?87J)y_o>0ZpA}~^ugSp_4&4~Yxndn_wFvxTyG=BSMyckuktknntPT3 zALW5YG~l#=%2Wb_^sqVwDit3``Y0?&^%lqL6iZIUqiQxn_Go|%aQddlh_;imMMQ=v zgz@3ELb^5E15}~YtLEQu(nL3Zp7DdTFP=SUexZ7d`6!j8gA)((sAYAo8-}L&-nHCa z-PgjgVKb19loeVb2HfQO_h4fImg2cCG|h>FBa=Py>NjV@j=TH+@WpD&M7 zb+-k-2EiT~?U69#AUGmcW(#LfF#8HCz-II9%2vV_^9&9^jS!^aIS zH{Ous8yo+)+HjN@-CsAJi`WvSLfOmksqtJ0P2VzK+XBf1$^7H;5UdwE8{|Ixp3!x+hp!@a+=4~gSKH;5?QG7+T4-kwo&XcRIYXO{7 zxRf-8UDimzs7)R#)+bn30YJ9uwil-jo%J$bpdv6TfJ`=c4^^O=!bupl<*Yy1wW_5t z%7t13;3L~yan|^4rX;4)b$&$l5b0(xij`uGRJR5>ftW&*%Es@Ev+z|=x`}tAXMh9z zL(*8afL7IvFrJC8@dG{JmonaDMx+Zq!G(MmaGZ;;?x@>Wj)i%D({DhuJdKJkK;ut6 zQBC#NSWMA4>f-$9aJDjAi97lgLcwe$!K10%T@5XcAD(o7h#R=bOT}3lX>6eHYIh&5 zjU%VTWo_{*uBBlNdo>DCJ;sw`56(SJ^~06y@%KwS(k?#wdgK&gd42cM!$A3?~FH%5lCAF@WKNC#W9i#QA>mdpASr9HrTFlr|}a#;M9V9 z(diWHc!RG9u;FUb&b2;Of{0-h2lwBR;?@4ui1Yi3{R$y#-54b`I)rh8s|hKQh%(eo zaDJB45>ctKQk?G5TPXo(lQHfPBU=C+@X8V?4R})fX1w++=bf=)+@NkR!{=fBjz=w$ zON|lRJ178|0N$4yrr+k_ofZi$fF&&h*7n=IwAbF)`3JgdOI*fsz{C z)zF5$F2Tkvm0I#sK>raN;{W()2UstbBvQcA9N479}ojK z01p6YCWbpU-VO+Xn}BBmG!sLG;eJ2@TnFqc9{4X2Q=DdmVk2E;P*7Pu8a^;mf{Cw_ z^9Wb8G2>}XiE7Ilx1DDWcE3!`mixgnMo2bST;?5XD_+BIWAt|wKB*?Z+zf=U7neX2SmD=?0Z^6C6=r7Ns}3y7 z7?l{Eg;aZaF1IQ7TY+FfADM;g2BR ztjr2Z+H%8-hPgyKA|LzWaUsSc`9e1{Q{M4Gj;i1?&5?6QCPq0%;wCtwULL`Ca)ujI zfS~|EQp+mJrH9pZchzpB!-y_o|tmK z7l>(!4+j`6y8Qhf-qHXu%}(FwmnPfviLqH`>cprSF7k&0PEb;46$qplRmuTRG3jNi zP{Je5B;1A@#C&dMy!)Cw>Bz>YO-QSDs(HuCxwK#(;HVJ(;p@a00B;vFP~a@r0W36! zrF1SRXMZsA3aM5S)3Q;*=ccEm-J)n^oW-fSQ zx@`(Zn=wFfe$Kfz#b@7)S1g3zV^jR6q-q$ofam!sCG|-_BQeZeQ~~8bsD>co#%=dr zt>^ET@V3i=wS-W};uq*1z156mR;0aCrKG~9A66-tpnFsbRUTgp5b;Nh zorSB%84q+U_)Z{y8@oP=l~9P8mXDPOf6&$B_=7*GB|vTAfJn6vD+r0vj?wa#MtVE6 za+#~|I#pIpaR~f{Wf>_QucKe#hu7O(lSja>5JFuYqeswq)<%CVjfrvpu==2z7;zla ziTQ74{+^jh@sHltkjpx3wP<(e+Bke!@UPF!3?|0?Bc@LBTSm0FLebp_7-LtfFOw%} zsO}0At7tVcmhcp|LOE0?;rp-HR1^JW*=M+yYBY9dtvz!dOx=u)JvRy~%?Q%Y^k$bL za4x2yz{)kD7UAG{fL&W!rZ zm9B&;#Nz=4i>Wp=RwtufvwqMK4CIoG9F<-Ut7v8%Mcjeo1y}l+#z?({AsJGz>L5P-fVMrwL)(5-O?WZ2l&>UAHLzcxmu*-LkCgaZ zQjff*9lzg>=tsP|V2z1Pbvx1iKmGWhe*6H{f2)3+q$xC1U(oOauOQxquEL17GS+XFkDsHuEkJ*6&G_?m z`oI;H|6Ob5|C8ztO;Hg2ET{(V6LhEbNva<>t|@4;^3bKHTCE=gXR6UcP64Pjg$5`B zo<)+dgb~bC{Xz`RsRXJgW=8cV<^467>M5as^{pZHT&ezQ?;46?A=j~K8LoBselQL| z-Eh|6i5$--xFR=27VHUO4bXxSq_J~lRRbWF+7)HEz~7i;NB5YwniyxJB)?59>^r;) zS^Bu&=nqp(X*T!k%-pine@s2y^kC#y^8NeDiZ21LA&Di+X=D3HIcN2_??-xzv0Vy^ z$by%wVE!!6Tp4){fmmzfaiX|+C~DcEs8v^Q(9g4fG4pVt#ruiJ2sHV+7G;~Enosey z%oweicFx|ZmIRv+&3*f(nL=j4HQdk`(ZgV|3=9*8r6a`8ASyI|{j{u0ikDv2P(9T} zu#phx{qU6xV|L1c>ds_i1^^!6KQqJ&UKC@oLs0Qt3Fbl)t z3O15oWnrPcquun*Igr^HGsnTf7wUm9GT8JxSGHX3BS7}v}K-cm{?UPuKE%A;_mekb4PJI-KMd*8runr6IP zKYl4yu4!iYsiEfK7C7!#W_4Xt1$l52V|qpO5f*G7nydlH_S&~+a2S(dJaHwG)t9MP zirMu=V`2A?@n^Bh+)-)H?Q^?=J4!{{$;bm0bD}n5kWYwnx&LANiYzW+`8sj03T?1V z7t@n}r*~+F(v`^=F3vNcoJ0)vxT;CP+M>ZP=7Z0k{|Wye7~@G# zXT?KeE=E%g2n%FC!vdqOGyi5F0emt})#~Gd+){XwO4@^V94ULv(Cq1+4tR>ETubp6 zEuiA!(~2=3_9SPW9pm?_RmH}NF;vEB0%|cPcNnQ?TZmaQ;jn+U7)b|U8P+^f2k9l9 z5ngE;64lHNiQ@#`Xf%L{7`E@+z4dvXAd&v0!bT91cYHDrk*WP{i4TA9udF`N`6K~q z6rf(&Ys>wch;c5#XcIic%SzD|5=t!)s-M7KSPHt~wovgQ-)~Aa9P9SxQUU5|0AXe( z6a~HAmlAIX_UKJXUX=n+^bmG;9ZjBOMkT))Fsm%ieW#aio$cV~iEck$XdR&X!4G}k z#Ar$z4b-jk0TC<#{8X0zgcFjy6mTt+-eZcqc%}o&u*Oqds!t@BOtGBUwKF4E7=hd{ zCGTQ;TbfN9#vP-BLrd~9lo`)uh*H4ow`m0ZwP*7cWZh*)dgcoz?Xv+-vGV3_vL9VA z-|L-BQuwrGa-W*C3f7~ql=5Fe^gKXZjUizx`~)LNML2{;kG`F-lj;EwVDj8FduW>`=l=F6`v8`Z_D+S z6Ep9{LAhH)VyMCek{D3>jq7`;_}Y-Yr2u?njCAKH zFxIL5T;(}6Mp!v8&>O!`Sv)w2r9DO;5EV|3(B_EG2f0Z#V7#A?pQy~4*0pwwe^+Hr zCW@^y-fU)NPHtvq0BZRNw+}Kq*(Mm+KKQ9Nt3g0)f$4t{l6CJ!ACRErm$mdsnE{n z9o2v`_^%MNl%CNDIMch3LsmmO<&+pFd-U$p@A?!#E#TIKQFBRZy_R-=UZ2Y>;xLYs zW#j~9PnSL)MsAxKezNiU>B3mwK7PY0gv!2FmC%5Z4Lv9qm@(EBCIA)jkN!Wsl!fKG z`>RShL~RD3mXBGbWLGbTv_k>3O0|S#=+k?uYy8(MfIdF{TM8hsrCyY@UjZD_!<%Z1 zF8=X=_iOwvHsrmI=_K8Lij1!T*n#oj9{Mn2e8lPn3rXD@h`l6sdJKTBo5o6vs>;dD zwkg-^uP8>O$B5n=KVhyrT0MG<^lY|Bb&mC8gyTB}Kki|PYfe+$eyB!?5s+P9CHgVy zVwL`=d$#|W!pB~gfDal{J(xfHFsAOrc&Iv{9%HC8o-<*ZT_NUIsFdGMJt{6C!-+WxN3R`*e@&q&2v4;3imBb#@;b%VW;eJvUkQIJU6T z^kZZlA7f7Gk4D**tvFStXEL$4@SOZTZk%^`O7k5p&05$0mu{S3RK~iIxI*{CP~1e> z{$CAE8ka(j`W+YnhRPEHL)ZJG$-7{?-lQz{!`EP&2Y8Bk@rNn9p#0;jjj<;;Kuj4F zi#x`|^f`vop9jGd0XvAPqU>ilKn=LIZN=f^QZvPC6xUwX-8D+*WiIWjs^j}TmN#zN%rT#f<3RB&x~oV9FnpC$_n{wSbMnMhQ@9*&CLEc8oE+ zg_8`x^RC0$m55UTWJ0fEw+HCoKZjD}DX|#K{dEu00UH|TtzVvVMHDe?IB&c#9BrRQ+!YwW09#qv54hWaa4&6ijC ztd+l4xoWw_YnNB6uq&*&|K_WHWe%uD3!rC^5kn5e~mKNDtK? zL6qY4czkN_BY^(v?~3PGjkTFf40W#FLC%1 zJ_r|l`kCQeBM~*s#OQL*YUetyRnAqSCkDTz^%8Zd$x37`TyN@AI-vp74Ou2WT}{cx~* zv0_bxBR<9i=4XCCDCW|9uzq;yB^Ns3jDt*DY!hfvPvMk2_G*jf<%67s>-#xZEQ&gkX7Dghh&VcKOWFWYy&kIp#mtkI1ooJ-Ya@Q zEyI<;4>Qi!Y93moXK%@(1Jq;0iv|`OIXXjP3Nuq2BZ5jRBPW#lA$jkNVtVx68H?cV z%+spHlI`^O6e=(iU>su_8dx(W!>v=;N_clusHzG)AzHD_$z%cb+HA%hQk;n-=1ftN zL#6hvPmNt4SHB?9g=J6J584hdunAQ;6)VN^QV7SWCq`8u-w=DP@7mW(OVcNPLjlF_ zJz#XW)GLR$NAD|>mjFlO9|_mGEc*@AR%R>D%>z8oO7S35QWdba9yH8_Sq!%UZuEpy z=m{$^8@gGPWhet4DMo6v>_ozdG+BAv(_imvoAUJu<2yd6@DVByfY!AhBigSIj2pR{ zPN@;wOn+jBGM3nJ`hbA+SnmWcWCY=PCE;O07rZ=DmoOu{4p4JGn?E!D8;)xA%qZdPF63)yF*XZq3&F4Zqw5^y zj8iDr`doZYYXWQ@YvvG>HS(Sw8lgv|onlg8twhCJ%|uA=Jj*#VymEfYEU* z78o$CYb_ubDb2`x8O=TLj*pcX!}*%uXQ;kVcm2gYbqZfNwJ+D}3q&N9F3>N+YoI)* z;}1t>coi@_q40o=#BV2rAIwiExUL(8r_&gLiNw5K;+w;_Ez7c^6wSgTp~iT9@u_QdF^Xyr`cP%>wZ>-WFv@^hXMw8W zeFjT91~j!bg+kSZg1kz^lV&4kqiZkypatA_n%bxZw2ZOE@;NTMu9AYY~^Wq+RW7FhK{kkknX0sA>uazcI<^)#q=S7lV(N*Nlv9>7V7Vw8HV8>^=U3m zt#_9^X1*!`W1PKk{3wqiTEKf_cW0buZm6Pl3V_>hsHb3@k8Bn~4CYLyN>{T$iBX-0q;BdMcC}9m1#nhUUJYYpgV=Y#*gY~_NsKyB zQXR}WMlhZ=6=}ryF~Lo@By4nX_$>+~Kcw<+J~~X_HTuhldWaY> z)$EfgW!Do{j1{k!l@#hQ=s`=RI*6Z~%?Fo>7Ii83cOXA@=GuT!Ktd&vto@cF z01|Mia*1J#d>(6&nrF#5dx-&@WQ>Be6m8}a;HG2B)SR<87sF#zhmnP1n=v>vSZ&a{ zz5w)MhEu+x26_~wYJ^Ca(vGz4*E`x?m=4ZxR!{=!yNm&F@oHM3}{lM z%d;$rO`BCowWGdFmn319z}ni;{^9(zo*F!Ct)>9WyzE*V!KNc-eu0{l4$v8Uy(zQP ztRXQTNy9)A)D#D(?;5KL*8RZ+uwXUOb*EXuhzn#Z0CyW;?-;rGOEc9>$mNS8D`dXm60*Q&s@jxnx~@+H<+ySA!j zUHSWKHPwp*{r6vV*A^87y#1PTpf*oNT-LG6rI=iLp*{!XZ)#vknTRDj%a2KW}00000NkvXXu0mjf_;Vpf 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..34c0b3db19 --- /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:6141114.8 %
Date:2024-06-08 22:13:12Functions:71936.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::speed_tracker::SpeedTracker::resetStatic()0
mrs_uav_trackers::speed_tracker::SpeedTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::callbackCommand(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const>)0
mrs_uav_trackers::speed_tracker::SpeedTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::getStatus()0
mrs_uav_trackers::speed_tracker::SpeedTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()20
(anonymous namespace)::ProxyExec0::ProxyExec0()107
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>)107
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)295
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)375
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
+
+
+ + + +
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..fae858e0c2 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-06-08 22:13:12Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()20
mrs_uav_trackers::speed_tracker::SpeedTracker::initialize(ros::NodeHandle const&, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t>)107
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&)375
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&)295
mrs_uav_trackers::speed_tracker::SpeedTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)6
mrs_uav_trackers::speed_tracker::SpeedTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)136027
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..06e59874ad --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html @@ -0,0 +1,1183 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6141114.8 %
Date:2024-06-08 22:13:12Functions: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         107 : 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         107 :   this->common_handlers_  = common_handlers;
+     129         107 :   this->private_handlers_ = private_handlers;
+     130             : 
+     131         107 :   _uav_name_ = common_handlers->uav_name;
+     132             : 
+     133         107 :   nh_ = nh;
+     134             : 
+     135         107 :   ros::Time::waitForValid();
+     136             : 
+     137             :   // --------------------------------------------------------------
+     138             :   // |                     loading parameters                     |
+     139             :   // --------------------------------------------------------------
+     140             : 
+     141             :   // | ---------------- load parent's parameters ---------------- |
+     142             : 
+     143         214 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     144             : 
+     145         107 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     146             : 
+     147         107 :   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         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/speed_tracker.yaml");
+     155         107 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/speed_tracker.yaml");
+     156             : 
+     157         214 :   const std::string yaml_prefix = "mrs_uav_trackers/speed_tracker/";
+     158             : 
+     159         107 :   private_handlers->param_loader->loadParam(yaml_prefix + "command_timeout", _external_command_timeout_);
+     160             : 
+     161         107 :   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         107 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "SpeedTracker", _profiler_enabled_);
+     169             : 
+     170             :   // | ----------------------- subscribers ---------------------- |
+     171             : 
+     172         107 :   mrs_lib::SubscribeHandlerOptions shopts;
+     173         107 :   shopts.nh              = nh_;
+     174         107 :   shopts.node_name       = "SpeedTracker";
+     175         107 :   shopts.threadsafe      = true;
+     176         107 :   shopts.autostart       = true;
+     177         107 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     178             : 
+     179         107 :   sh_command_ = mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand>(shopts, "command", &SpeedTracker::callbackCommand, this);
+     180             : 
+     181             :   // | ----------------------- publishers ----------------------- |
+     182             : 
+     183         107 :   ph_rviz_marker_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "rviz_marker", 1);
+     184             : 
+     185             :   // | --------------------- finish the init -------------------- |
+     186             : 
+     187         107 :   is_initialized_ = true;
+     188             : 
+     189         107 :   ROS_INFO("[SpeedTracker]: initialized");
+     190             : 
+     191         107 :   return true;
+     192             : }
+     193             : 
+     194             : //}
+     195             : 
+     196             : /* //{ activate() */
+     197             : 
+     198           0 : std::tuple<bool, std::string> SpeedTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     199             : 
+     200           0 :   std::stringstream ss;
+     201             : 
+     202           0 :   if (!got_uav_state_) {
+     203           0 :     ss << "odometry not set";
+     204           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     205           0 :     return std::tuple(false, ss.str());
+     206             :   }
+     207             : 
+     208           0 :   if (!sh_command_.hasMsg()) {
+     209           0 :     ss << "missing command";
+     210           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     211           0 :     return std::tuple(false, ss.str());
+     212             :   }
+     213             : 
+     214           0 :   ros::Time external_command_time = sh_command_.lastMsgTime();
+     215             : 
+     216             :   // timeout the external command
+     217           0 :   if ((ros::Time::now() - external_command_time).toSec() > _external_command_timeout_) {
+     218           0 :     ss << "the command is too old";
+     219           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     220           0 :     return std::tuple(false, ss.str());
+     221             :   }
+     222             : 
+     223           0 :   is_active_ = true;
+     224             : 
+     225           0 :   ss << "activated";
+     226           0 :   ROS_INFO_STREAM("[SpeedTracker]: " << ss.str());
+     227             : 
+     228           0 :   return std::tuple(true, ss.str());
+     229             : }
+     230             : 
+     231             : //}
+     232             : 
+     233             : /* //{ deactivate() */
+     234             : 
+     235          20 : void SpeedTracker::deactivate(void) {
+     236             : 
+     237          20 :   is_active_ = false;
+     238             : 
+     239          20 :   ROS_INFO("[SpeedTracker]: deactivated");
+     240          20 : }
+     241             : 
+     242             : //}
+     243             : 
+     244             : /* //{ resetStatic() */
+     245             : 
+     246           0 : bool SpeedTracker::resetStatic(void) {
+     247             : 
+     248           0 :   return false;
+     249             : }
+     250             : 
+     251             : //}
+     252             : 
+     253             : /* //{ update() */
+     254             : 
+     255      136027 : 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      408081 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     259      408081 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     260             : 
+     261             :   {
+     262      136027 :     std::scoped_lock lock(mutex_uav_state_);
+     263             : 
+     264      136027 :     uav_state_ = uav_state;
+     265             : 
+     266      136027 :     got_uav_state_ = true;
+     267             :   }
+     268             : 
+     269             :   double uav_heading;
+     270             : 
+     271             :   try {
+     272      136027 :     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      136027 :   if (!is_active_) {
+     282      136027 :     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 :   if (!tracker_status.active || first_iteration_)
+     374             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+     375             :   else
+     376             :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+     377             : 
+     378             :   return tracker_status;
+     379             : }
+     380         295 : 
+     381             : //}
+     382         590 : 
+     383         590 : /* //{ enableCallbacks() */
+     384             : 
+     385         295 : const std_srvs::SetBoolResponse::ConstPtr SpeedTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     386             : 
+     387          19 :   std_srvs::SetBoolResponse res;
+     388             :   std::stringstream         ss;
+     389          19 : 
+     390          19 :   if (cmd->data != callbacks_enabled_) {
+     391             : 
+     392             :     callbacks_enabled_ = cmd->data;
+     393             : 
+     394         276 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     395         276 :     ROS_INFO_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     396             : 
+     397             :   } else {
+     398         295 : 
+     399         295 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     400             :     ROS_WARN_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     401         590 :   }
+     402             : 
+     403             :   res.message = ss.str();
+     404             :   res.success = true;
+     405             : 
+     406             :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     407             : }
+     408           0 : 
+     409             : //}
+     410           0 : 
+     411             : /* switchOdometrySource() //{ */
+     412             : 
+     413             : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     414             : 
+     415             :   return std_srvs::TriggerResponse::Ptr();
+     416             : }
+     417           0 : 
+     418             : //}
+     419           0 : 
+     420             : /* //{ hover() */
+     421             : 
+     422             : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     423             : 
+     424             :   return std_srvs::TriggerResponse::Ptr();
+     425             : }
+     426           0 : 
+     427           0 : //}
+     428             : 
+     429             : /* //{ startTrajectoryTracking() */
+     430             : 
+     431             : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     432             :   return std_srvs::TriggerResponse::Ptr();
+     433             : }
+     434           0 : 
+     435           0 : //}
+     436             : 
+     437             : /* //{ stopTrajectoryTracking() */
+     438             : 
+     439             : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     440             :   return std_srvs::TriggerResponse::Ptr();
+     441             : }
+     442           0 : 
+     443           0 : //}
+     444             : 
+     445             : /* //{ resumeTrajectoryTracking() */
+     446             : 
+     447             : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     448             :   return std_srvs::TriggerResponse::Ptr();
+     449             : }
+     450           0 : 
+     451           0 : //}
+     452             : 
+     453             : /* //{ gotoTrajectoryStart() */
+     454             : 
+     455             : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     456             :   return std_srvs::TriggerResponse::Ptr();
+     457             : }
+     458         375 : 
+     459             : //}
+     460             : 
+     461             : /* //{ setConstraints() */
+     462         375 : 
+     463             : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr SpeedTracker::setConstraints([
+     464         375 :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     465             : 
+     466             :   {
+     467         750 :     std::scoped_lock lock(mutex_constraints_);
+     468             : 
+     469         375 :     constraints_ = cmd->constraints;
+     470         375 :   }
+     471             : 
+     472         750 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     473             : 
+     474             :   res.success = true;
+     475             :   res.message = "constraints updated";
+     476             : 
+     477             :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     478             : }
+     479           0 : 
+     480             : //}
+     481           0 : 
+     482             : /* //{ setReference() */
+     483             : 
+     484             : const mrs_msgs::ReferenceSrvResponse::ConstPtr SpeedTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     485             : 
+     486             :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     487             : }
+     488           0 : 
+     489             : //}
+     490           0 : 
+     491             : /* //{ setVelocityReference() */
+     492             : 
+     493             : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr SpeedTracker::setVelocityReference([
+     494             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     495             :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     496             : }
+     497           6 : 
+     498             : //}
+     499           6 : 
+     500             : /* //{ setTrajectoryReference() */
+     501             : 
+     502             : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr SpeedTracker::setTrajectoryReference([
+     503             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     504             :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     505             : }
+     506             : 
+     507             : //}
+     508           0 : 
+     509             : // | --------------------- custom methods --------------------- |
+     510           0 : 
+     511           0 : /* callbackCommand() //{ */
+     512             : 
+     513           0 : void SpeedTracker::callbackCommand(const mrs_msgs::SpeedTrackerCommand::ConstPtr msg) {
+     514           0 : 
+     515             :   if (!is_initialized_)
+     516           0 :     return;
+     517             : 
+     518             :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackCommand");
+     519           0 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::callbackCommand", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     520             : 
+     521           0 :   mrs_msgs::SpeedTrackerCommandConstPtr external_command = msg;
+     522           0 : 
+     523             :   double dt;
+     524             :   if (first_iteration_) {
+     525           0 : 
+     526             :     last_command_time_ = ros::Time::now();
+     527           0 :     first_iteration_   = false;
+     528             : 
+     529             :     {
+     530           0 :       std::scoped_lock lock(mutex_command_);
+     531             : 
+     532           0 :       command_ = *external_command;
+     533           0 :     }
+     534             : 
+     535           0 :     return;
+     536           0 :   } else {
+     537           0 :     dt                 = (ros::Time::now() - last_command_time_).toSec();
+     538             :     last_command_time_ = ros::Time::now();
+     539             : 
+     540             :     if (dt <= 1e-4) {
+     541           0 :       ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: the command dt is %.5f, returning", dt);
+     542             :       return;
+     543           0 :     }
+     544           0 :   }
+     545           0 : 
+     546             :   mrs_msgs::SpeedTrackerCommand transformed_command = *external_command;
+     547             : 
+     548             :   auto old_command = mrs_lib::get_mutexed(mutex_command_, command_);
+     549             :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     550           0 :   auto uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     551             : 
+     552           0 :   double uav_heading;
+     553           0 : 
+     554           0 :   try {
+     555             :     uav_heading = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+     556             :   }
+     557             :   catch (...) {
+     558             :     ROS_ERROR_THROTTLE(1.0, "[SpeedTracker]: could not calculate UAV heading");
+     559             :     return;
+     560             :   }
+     561           0 : 
+     562             :   // transform the command
+     563           0 : 
+     564           0 :   // transform velocity
+     565             : 
+     566           0 :   if (transformed_command.use_velocity) {
+     567           0 : 
+     568           0 :     geometry_msgs::Vector3Stamped vector3;
+     569             :     vector3.header = transformed_command.header;
+     570           0 : 
+     571             :     vector3.vector.x = transformed_command.velocity.x;
+     572           0 :     vector3.vector.y = transformed_command.velocity.y;
+     573           0 :     vector3.vector.z = transformed_command.velocity.z;
+     574           0 : 
+     575           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     576             : 
+     577           0 :     if (ret) {
+     578             :       transformed_command.velocity.x = ret.value().vector.x;
+     579             :       transformed_command.velocity.y = ret.value().vector.y;
+     580             :       transformed_command.velocity.z = ret.value().vector.z;
+     581             :     } else {
+     582             :       return;
+     583           0 :     }
+     584             : 
+     585           0 :     /* horizontal speed limit //{ */
+     586             : 
+     587           0 :     {
+     588             :       double des_horizontal_speed = sqrt(pow(transformed_command.velocity.x, 2) + pow(transformed_command.velocity.y, 2));
+     589           0 : 
+     590           0 :       if (des_horizontal_speed > constraints.horizontal_speed) {
+     591             : 
+     592             :         double des_speed_heading = atan2(transformed_command.velocity.y, transformed_command.velocity.x);
+     593             : 
+     594             :         transformed_command.velocity.x = cos(des_speed_heading) * constraints.horizontal_speed;
+     595             :         transformed_command.velocity.y = sin(des_speed_heading) * constraints.horizontal_speed;
+     596             :       }
+     597             :     }
+     598             : 
+     599             :     //}
+     600           0 : 
+     601             :     /* horizontal speed change rate limit //{ */
+     602             : 
+     603           0 :     {
+     604             :       Eigen::Vector2d hor_speed_derivative =
+     605           0 :           Eigen::Vector2d(transformed_command.velocity.x - old_command.velocity.x, transformed_command.velocity.y - old_command.velocity.y) / dt;
+     606           0 : 
+     607             :       // exceeding the maximum acceleration
+     608           0 :       if (hor_speed_derivative.norm() > constraints.horizontal_acceleration) {
+     609           0 : 
+     610             :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting speed change rate");
+     611             :         double direction = atan2(hor_speed_derivative[1], hor_speed_derivative[0]);
+     612             : 
+     613             :         transformed_command.velocity.x = old_command.velocity.x + cos(direction) * constraints.horizontal_acceleration * dt;
+     614             :         transformed_command.velocity.y = old_command.velocity.y + sin(direction) * constraints.horizontal_acceleration * dt;
+     615             :       }
+     616             :     }
+     617             : 
+     618             :     //}
+     619           0 : 
+     620             :     /* vertical speed limit //{ */
+     621           0 : 
+     622             :     {
+     623             :       // if ascending
+     624             :       if (transformed_command.velocity.z > constraints.vertical_ascending_speed) {
+     625           0 : 
+     626             :         transformed_command.velocity.z = constraints.vertical_ascending_speed;
+     627           0 :       }
+     628             : 
+     629             :       // if descending
+     630             :       if (transformed_command.velocity.z < -constraints.vertical_descending_speed) {
+     631             : 
+     632             :         transformed_command.velocity.z = -constraints.vertical_descending_speed;
+     633             :       }
+     634             :     }
+     635             : 
+     636             :     //}
+     637           0 : 
+     638             :     /* vertical speed change rate //{ */
+     639           0 : 
+     640             :     {
+     641           0 : 
+     642           0 :       double vert_speed_derivative = (transformed_command.velocity.z - old_command.velocity.z) / dt;
+     643             : 
+     644           0 :       if (vert_speed_derivative > constraints.vertical_ascending_acceleration) {
+     645             : 
+     646           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical ascending speed change rate");
+     647           0 :         transformed_command.velocity.z = old_command.velocity.z + constraints.vertical_ascending_acceleration * dt;
+     648             : 
+     649             :       } else if (vert_speed_derivative < -constraints.vertical_descending_acceleration) {
+     650             : 
+     651             :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical descending speed change rate");
+     652             :         transformed_command.velocity.z = old_command.velocity.z - constraints.vertical_descending_acceleration * dt;
+     653             :       }
+     654             :     }
+     655             : 
+     656           0 :     //}
+     657             :   }
+     658           0 : 
+     659           0 :   /* transform and constrain heading //{ */
+     660             : 
+     661           0 :   if (transformed_command.use_heading) {
+     662             : 
+     663           0 :     mrs_msgs::ReferenceStamped temp_ref;
+     664             :     temp_ref.header = transformed_command.header;
+     665           0 : 
+     666             :     temp_ref.reference.heading = transformed_command.heading;
+     667             : 
+     668           0 :     auto ret = common_handlers_->transformer->transformSingle(temp_ref, "");
+     669             : 
+     670             :     if (ret) {
+     671           0 : 
+     672             :       // calculate the produced heading rate
+     673           0 :       double des_hdg_rate = sradians::diff(ret.value().reference.heading, old_command.heading) / dt;
+     674           0 : 
+     675             :       // saturate the change in the desired heading
+     676           0 :       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 if (des_hdg_rate < -constraints.heading_speed) {
+     682             : 
+     683           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting change of the desired heading using constraints");
+     684             :         transformed_command.heading = old_command.heading - constraints.heading_speed * dt;
+     685             : 
+     686             :       } else {
+     687           0 : 
+     688             :         transformed_command.heading = ret.value().reference.heading;
+     689             :       }
+     690           0 : 
+     691           0 :     } else {
+     692             :       return;
+     693             :     }
+     694             :   } else {
+     695             :     transformed_command.use_heading = false;
+     696           0 :     transformed_command.heading     = uav_heading;
+     697             :   }
+     698           0 : 
+     699           0 :   //}
+     700           0 : 
+     701           0 :   if (transformed_command.use_heading_rate) {
+     702             : 
+     703             :     if (transformed_command.heading_rate > constraints.heading_speed) {
+     704             :       transformed_command.heading_rate = constraints.heading_speed;
+     705           0 :     } else if (transformed_command.heading_rate < -constraints.heading_speed) {
+     706             :       transformed_command.heading_rate = -constraints.heading_speed;
+     707           0 :     }
+     708           0 :   }
+     709             : 
+     710           0 :   if (transformed_command.use_acceleration) {
+     711           0 : 
+     712           0 :     geometry_msgs::Vector3Stamped vector3;
+     713             :     vector3.header = transformed_command.header;
+     714           0 : 
+     715             :     vector3.vector.x = transformed_command.acceleration.x;
+     716           0 :     vector3.vector.y = transformed_command.acceleration.y;
+     717           0 :     vector3.vector.z = transformed_command.acceleration.z;
+     718           0 : 
+     719           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     720             : 
+     721           0 :     if (ret) {
+     722             :       transformed_command.acceleration.x = ret.value().vector.x;
+     723             :       transformed_command.acceleration.y = ret.value().vector.y;
+     724             :       transformed_command.acceleration.z = ret.value().vector.z;
+     725             :     } else {
+     726             :       return;
+     727           0 :     }
+     728             : 
+     729           0 :     /* horizontal acceleration limit //{ */
+     730             : 
+     731           0 :     {
+     732             :       double des_horizontal_acceleration = sqrt(pow(transformed_command.acceleration.x, 2) + pow(transformed_command.acceleration.y, 2));
+     733           0 : 
+     734           0 :       if (des_horizontal_acceleration > constraints.horizontal_acceleration) {
+     735             : 
+     736             :         double des_acc_heading = atan2(transformed_command.acceleration.y, transformed_command.acceleration.x);
+     737             : 
+     738             :         transformed_command.acceleration.x = cos(des_acc_heading) * constraints.horizontal_acceleration;
+     739             :         transformed_command.acceleration.y = sin(des_acc_heading) * constraints.horizontal_acceleration;
+     740             :       }
+     741             :     }
+     742             : 
+     743             :     //}
+     744           0 : 
+     745           0 :     /* horizontal acceleration change rate limit //{ */
+     746             : 
+     747             :     {
+     748           0 :       Eigen::Vector2d hor_acc_derivative =
+     749             :           Eigen::Vector2d(transformed_command.acceleration.x - old_command.acceleration.x, transformed_command.acceleration.y - old_command.acceleration.y) /
+     750           0 :           (dt);
+     751           0 : 
+     752             :       // exceeding the maximum acceleration
+     753           0 :       if (hor_acc_derivative.norm() > constraints.horizontal_jerk) {
+     754           0 : 
+     755             :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting acceleration change rate");
+     756             :         double direction = atan2(hor_acc_derivative[1], hor_acc_derivative[0]);
+     757             : 
+     758             :         transformed_command.acceleration.x = old_command.acceleration.x + cos(direction) * constraints.horizontal_jerk * dt;
+     759             :         transformed_command.acceleration.y = old_command.acceleration.y + sin(direction) * constraints.horizontal_jerk * dt;
+     760             :       }
+     761             :     }
+     762             : 
+     763             :     //}
+     764           0 : 
+     765             :     /* vertical acceleration limit //{ */
+     766           0 : 
+     767             :     {
+     768             :       // if ascending
+     769             :       if (transformed_command.acceleration.z > constraints.vertical_ascending_acceleration) {
+     770           0 : 
+     771             :         transformed_command.acceleration.z = constraints.vertical_ascending_acceleration;
+     772           0 :       }
+     773             : 
+     774             :       // if descending
+     775             :       if (transformed_command.acceleration.z < -constraints.vertical_descending_acceleration) {
+     776             : 
+     777             :         transformed_command.acceleration.z = -constraints.vertical_descending_acceleration;
+     778             :       }
+     779             :     }
+     780             : 
+     781             :     //}
+     782           0 : 
+     783             :     /* vertical acceleration change rate //{ */
+     784           0 : 
+     785             :     {
+     786           0 : 
+     787           0 :       double vert_acc_derivative = (transformed_command.acceleration.z - old_command.acceleration.z) / dt;
+     788             : 
+     789           0 :       if (vert_acc_derivative > constraints.vertical_ascending_jerk) {
+     790             : 
+     791           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical ascending acceleration change rate");
+     792           0 :         transformed_command.acceleration.z = old_command.acceleration.z + constraints.vertical_ascending_jerk * dt;
+     793             : 
+     794             :       } else if (vert_acc_derivative < -constraints.vertical_descending_jerk) {
+     795             : 
+     796             :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical descending acceleration change rate");
+     797             :         transformed_command.acceleration.z = old_command.acceleration.z - constraints.vertical_descending_jerk * dt;
+     798             :       }
+     799             :     }
+     800             : 
+     801           0 :     //}
+     802             :   }
+     803           0 : 
+     804           0 :   // transform force
+     805             : 
+     806           0 :   if (transformed_command.use_force) {
+     807           0 : 
+     808           0 :     geometry_msgs::Vector3Stamped vector3;
+     809             :     vector3.header = transformed_command.header;
+     810           0 : 
+     811             :     vector3.vector.x = transformed_command.force.x;
+     812           0 :     vector3.vector.y = transformed_command.force.y;
+     813           0 :     vector3.vector.z = transformed_command.force.z;
+     814           0 : 
+     815           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     816             : 
+     817           0 :     if (ret) {
+     818             :       transformed_command.force.x = vector3.vector.x;
+     819             :       transformed_command.force.y = vector3.vector.y;
+     820             :       transformed_command.force.z = vector3.vector.z;
+     821             :     } else {
+     822             :       return;
+     823           0 :     }
+     824             :   }
+     825           0 : 
+     826             :   // check the feasibility of the z
+     827           0 :   {
+     828             :     double z_derivative = (transformed_command.z - old_command.z) / dt;
+     829           0 : 
+     830             :     if (z_derivative > constraints.vertical_ascending_speed) {
+     831           0 : 
+     832             :       transformed_command.z = old_command.z + constraints.vertical_ascending_speed * dt;
+     833             : 
+     834             :     } else if (z_derivative < -constraints.vertical_ascending_speed) {
+     835           0 : 
+     836             :       transformed_command.z = old_command.z - constraints.vertical_descending_speed * dt;
+     837           0 :     }
+     838             : 
+     839           0 :     // saturate the desired z using the safety area
+     840             :     if (common_handlers_->safety_area.use_safety_area) {
+     841           0 : 
+     842             :       if (transformed_command.z > common_handlers_->safety_area.getMaxZ("")) {
+     843           0 : 
+     844             :         transformed_command.z = common_handlers_->safety_area.getMaxZ("");
+     845             : 
+     846             :       } else if (transformed_command.z < common_handlers_->safety_area.getMinZ("")) {
+     847             : 
+     848             :         transformed_command.z = common_handlers_->safety_area.getMinZ("");
+     849             :       }
+     850           0 :     }
+     851             :   }
+     852           0 : 
+     853             :   // if not active, nullify the desired speeds and accelerations
+     854           0 :   // this will produce a rumpum (using the constraints) after the activation
+     855           0 :   if (!is_active_) {
+     856           0 : 
+     857             :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     858           0 : 
+     859           0 :     transformed_command.velocity.x = 0;
+     860           0 :     transformed_command.velocity.y = 0;
+     861             :     transformed_command.velocity.z = 0;
+     862             : 
+     863           0 :     transformed_command.acceleration.x = 0;
+     864             :     transformed_command.acceleration.y = 0;
+     865           0 :     transformed_command.acceleration.z = 0;
+     866           0 : 
+     867             :     try {
+     868             :       transformed_command.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     869           0 :     }
+     870             :     catch (...) {
+     871             :       return;
+     872             :     }
+     873           0 : 
+     874             :     transformed_command.z = uav_state_.pose.position.z;
+     875           0 :   }
+     876             : 
+     877             :   {
+     878           0 :     std::scoped_lock lock(mutex_command_);
+     879           0 : 
+     880             :     command_ = transformed_command;
+     881           0 :   }
+     882             : 
+     883             :   if (!is_active_) {
+     884             :     ROS_INFO_ONCE("[SpeedTracker]: getting command");
+     885             :   } else {
+     886             :     ROS_INFO_THROTTLE(5.0, "[SpeedTracker]: getting command");
+     887             :   }
+     888           0 : 
+     889             :   // --------------------------------------------------------------
+     890           0 :   // |                     publish rviz markers                   |
+     891             :   // --------------------------------------------------------------
+     892           0 : 
+     893             :   visualization_msgs::MarkerArray msg_out;
+     894             : 
+     895             :   double id = 0;
+     896           0 : 
+     897             :   geometry_msgs::Point point;
+     898           0 : 
+     899             :   /* desired speed //{ */
+     900           0 : 
+     901             :   if (transformed_command.use_velocity) {
+     902           0 : 
+     903           0 :     std::scoped_lock lock(mutex_uav_state_);
+     904           0 : 
+     905           0 :     visualization_msgs::Marker marker;
+     906           0 : 
+     907           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+     908             :     marker.header.stamp    = ros::Time::now();
+     909             :     marker.ns              = "speed_tracker";
+     910             :     marker.id              = id++;
+     911           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+     912           0 :     marker.action          = visualization_msgs::Marker::ADD;
+     913           0 : 
+     914             :     /* position //{ */
+     915             : 
+     916             :     marker.pose.position.x = 0.0;
+     917             :     marker.pose.position.y = 0.0;
+     918             :     marker.pose.position.z = 0.0;
+     919           0 : 
+     920             :     //}
+     921             : 
+     922             :     /* orientation //{ */
+     923             : 
+     924           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     925           0 : 
+     926           0 :     //}
+     927             : 
+     928           0 :     /* origin //{ */
+     929             :     point.x = uav_state_.pose.position.x;
+     930             :     point.y = uav_state_.pose.position.y;
+     931             :     point.z = uav_state_.pose.position.z;
+     932             : 
+     933             :     marker.points.push_back(point);
+     934           0 : 
+     935           0 :     //}
+     936           0 : 
+     937             :     /* tip //{ */
+     938           0 : 
+     939             :     point.x = uav_state_.pose.position.x + transformed_command.velocity.x;
+     940             :     point.y = uav_state_.pose.position.y + transformed_command.velocity.y;
+     941             :     point.z = uav_state_.pose.position.z + transformed_command.velocity.z;
+     942           0 : 
+     943           0 :     marker.points.push_back(point);
+     944           0 : 
+     945             :     //}
+     946           0 : 
+     947           0 :     marker.scale.x = 0.05;
+     948           0 :     marker.scale.y = 0.05;
+     949           0 :     marker.scale.z = 0.05;
+     950             : 
+     951           0 :     marker.color.a = 0.5;
+     952             :     marker.color.r = 0.0;
+     953           0 :     marker.color.g = 1.0;
+     954             :     marker.color.b = 0.0;
+     955             : 
+     956             :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+     957             : 
+     958             :     msg_out.markers.push_back(marker);
+     959           0 :   }
+     960             : 
+     961           0 :   //}
+     962             : 
+     963           0 :   /* desired acceleration //{ */
+     964             :   if (transformed_command.use_acceleration) {
+     965           0 : 
+     966           0 :     std::scoped_lock lock(mutex_uav_state_);
+     967           0 : 
+     968           0 :     visualization_msgs::Marker marker;
+     969           0 : 
+     970           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+     971             :     marker.header.stamp    = ros::Time::now();
+     972             :     marker.ns              = "speed_tracker";
+     973             :     marker.id              = id++;
+     974           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+     975           0 :     marker.action          = visualization_msgs::Marker::ADD;
+     976           0 : 
+     977             :     /* position //{ */
+     978             : 
+     979             :     marker.pose.position.x = 0.0;
+     980             :     marker.pose.position.y = 0.0;
+     981             :     marker.pose.position.z = 0.0;
+     982           0 : 
+     983             :     //}
+     984             : 
+     985             :     /* orientation //{ */
+     986             : 
+     987           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     988           0 : 
+     989           0 :     //}
+     990             : 
+     991           0 :     /* origin //{ */
+     992             :     point.x = uav_state_.pose.position.x;
+     993             :     point.y = uav_state_.pose.position.y;
+     994             :     point.z = uav_state_.pose.position.z;
+     995             : 
+     996             :     marker.points.push_back(point);
+     997           0 : 
+     998           0 :     //}
+     999           0 : 
+    1000             :     /* tip //{ */
+    1001           0 : 
+    1002             :     point.x = uav_state_.pose.position.x + transformed_command.acceleration.x;
+    1003             :     point.y = uav_state_.pose.position.y + transformed_command.acceleration.y;
+    1004             :     point.z = uav_state_.pose.position.z + transformed_command.acceleration.z;
+    1005           0 : 
+    1006           0 :     marker.points.push_back(point);
+    1007           0 : 
+    1008             :     //}
+    1009           0 : 
+    1010           0 :     marker.scale.x = 0.05;
+    1011           0 :     marker.scale.y = 0.05;
+    1012           0 :     marker.scale.z = 0.05;
+    1013             : 
+    1014           0 :     marker.color.a = 0.5;
+    1015             :     marker.color.r = 1.0;
+    1016           0 :     marker.color.g = 0.0;
+    1017             :     marker.color.b = 0.0;
+    1018             : 
+    1019             :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1020             : 
+    1021             :     msg_out.markers.push_back(marker);
+    1022           0 :   }
+    1023             : 
+    1024           0 :   //}
+    1025             : 
+    1026           0 :   /* desired force //{ */
+    1027             :   if (transformed_command.use_force) {
+    1028           0 : 
+    1029           0 :     std::scoped_lock lock(mutex_uav_state_);
+    1030           0 : 
+    1031           0 :     visualization_msgs::Marker marker;
+    1032           0 : 
+    1033           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+    1034             :     marker.header.stamp    = ros::Time::now();
+    1035             :     marker.ns              = "speed_tracker";
+    1036             :     marker.id              = id++;
+    1037           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+    1038           0 :     marker.action          = visualization_msgs::Marker::ADD;
+    1039           0 : 
+    1040             :     /* position //{ */
+    1041             : 
+    1042             :     marker.pose.position.x = 0.0;
+    1043             :     marker.pose.position.y = 0.0;
+    1044             :     marker.pose.position.z = 0.0;
+    1045           0 : 
+    1046             :     //}
+    1047             : 
+    1048             :     /* orientation //{ */
+    1049             : 
+    1050           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1051           0 : 
+    1052           0 :     //}
+    1053             : 
+    1054           0 :     /* origin //{ */
+    1055             :     point.x = uav_state_.pose.position.x;
+    1056             :     point.y = uav_state_.pose.position.y;
+    1057             :     point.z = uav_state_.pose.position.z;
+    1058             : 
+    1059             :     marker.points.push_back(point);
+    1060           0 : 
+    1061           0 :     //}
+    1062           0 : 
+    1063             :     /* tip //{ */
+    1064           0 : 
+    1065             :     point.x = uav_state_.pose.position.x + transformed_command.force.x;
+    1066             :     point.y = uav_state_.pose.position.y + transformed_command.force.y;
+    1067             :     point.z = uav_state_.pose.position.z + transformed_command.force.z;
+    1068           0 : 
+    1069           0 :     marker.points.push_back(point);
+    1070           0 : 
+    1071             :     //}
+    1072           0 : 
+    1073           0 :     marker.scale.x = 0.05;
+    1074           0 :     marker.scale.y = 0.05;
+    1075           0 :     marker.scale.z = 0.05;
+    1076             : 
+    1077           0 :     marker.color.a = 0.5;
+    1078             :     marker.color.r = 0.0;
+    1079           0 :     marker.color.g = 0.0;
+    1080             :     marker.color.b = 1.0;
+    1081             : 
+    1082             :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1083             : 
+    1084           0 :     msg_out.markers.push_back(marker);
+    1085             :   }
+    1086             : 
+    1087             :   //}
+    1088             : 
+    1089             :   ph_rviz_marker_.publish(msg_out);
+    1090             : }
+    1091             : 
+    1092             : //}
+    1093             : 
+    1094         107 : }  // namespace speed_tracker
+    1095             : 
+    1096             : }  // namespace mrs_uav_trackers
+    1097             : 
+    1098             : #include <pluginlib/class_list_macros.h>
+    1099             : 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..4d8a2bb401 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html @@ -0,0 +1,295 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ 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..25c6c3a9c4dc4bd8eb422cf3f8644d7dad9f6a32 GIT binary patch literal 3298 zcmV<83?1`{P))ExV|Thj5nW_qYm$xc3mev zq3gw3q-9|UMK8*0wAW<3*9?dbVhm<${9VMiEB!#VsibkjB;7O9V@6fBaRGQj_l$(= zC1Z58Jgy0bd^)F?3|t~RdD#Sd4phRF6g>qZjR-uzWPGZ?YXUsWV@6Tqtg;@H_Ys93 z|Ac@G3Va~};Ocn9g+|e_1BFf-6r(Y%=acJ;J&pTRtb|# z{;$cr@IqFssZxZ;uQ% zdppwBNstJ$zqI{G4ey%L_;(Mlik1NH0eEMCU~?32iRdLg9mOYS9Ep=W%4*S5ys)8;I(D?P zI80$0Cgw_jAVs**Nz)acY5dVU4bfv}>goHvBa_pK#-yRhvv$jY+xDPsfBoGb=K06A z%{2>r{N`LS;`hELoEFE!^V6kjQz)R{t{=LNYMhRtqQ=q1;P~Dz@L1MO_|X8j5LO^ zog2b)>uWD{i_2af{Q_ed#A7X4>5OJx^3xP6))j$3wq=2Eo3ef7OluTH61<2@JmbV{OdlRLsr#=M zBRJ2J$P^}=NH#C%{w7;%KF!tip>2Oh$3(AkDt(G%s-5C?1s!~4D$ zMn5vK>jqHrecv_E@Af|gfGu&?3UIuIr(M(Tf7c#EX{f&K@ehy0?`~gNe!Y zrLLYHKh_--Eg@%T)vkk`@vZ=OooqH$mH->uYk@Bww5g4D+>~a|f2jwX9G|G-VB^A; z;m?IQa%Rsi?O}?ySXsLbDTn0+OkW+$-|4T}w}FQswc{pu*S1bb%8zXMMN#^^WafA} z1U)6a-CY4PD%uw2e0k3CD2$5#oL-n$jcTZc6JZYi?uqJe;ip2F;XdOY2CAHf_bmX& zm1o5#0MeKScL>Mu_7+Zv;V4B~A$k50feP)kpLSD*d`{sU?n4xAPZhL)3hh7&hhjt^ z`F9i^H)x$6PR9YFhbaLFM$7ce$+PYLhu{grYy`y{NB%a)_ z1Gg3=0Ku+9v5?$V#aR=n*&F82Q=imDi)Zb2Qi@km1dd7hLtoZKsy2*w9i@pTTnBE_ z?wSXJUBjNekppK@=V!Wh6!(_-K7rU|s8l%Q3W{A)|2hhk?Gc$xGuDcB-0IkVhH*=F zXyc;pt2k0O-M!#5y%<^!u)53`_4?sQ~E(b^iS0qu4Vi9P6d7 z9DD52WBW|AqnN9gZ;D*O<|hc8(XYGB)V>OPRe9bbwc*LfMo)Lw^{A!Tp4fF16DvHq zYtOl16_~lgh^`Conh#X)xN}DZVmopb6~!HylCof*3q^pbcjaMCbi*nT-FQX)bxH={ z&()jS{+@;+&f|k7QG7(*RS$@y2l6s|+bIyE=!RzDMw-N5?Zs}g`K)Bx#DQoO|FzYt zVhE`{9yiIbR*7?{v`5!@VKXU^Ih|hGT!-QA@ktkMvdb-8@S0t>`rxx+Itb4|X-C_+ zLhc5%wxYD3uPewSV{@KHWS$Y4FZO}AD>5k+bu%tTcH1Kt#~+71ZlE-(VDF@E8!;xe zssRN5vW2@_!_E!ij07poQc7#IZ**rV(n4|{7Q-KYZfQAJU`6FKtA7->Ww;7MBh=jQ z?5flCGiHUSy1^hdp)f=rv$kmkSGzy8Y-Sf=-OFb2?xG0HyoEWM-r03oqI5vbShp6r z<|UrSxsKuv(lsn~Viv~p!~#PiDnuxwiWFOj_D8Vy<hX?)Bc^6h@gcdN%o&rBH~ ziRe@dz438PHz7N2YraenM$@p0M(@Zlwnp>_s|V~F+ta{z0Tk1WrY@9P3_q9VT9h92 zn=;P~Pr10AV5UH;4Wx3eKwYtc3kpvk^X1Lg7^eQnmwtbD4{5Z*-?JVrJ<&Z5sNnH! z>fwAvoO1=S(2W8=svd4f;e6KITMriuu79QZIA1MvL$ff$Oa}jC|GTX*3+_k_Kp>w3|}jg~bFzHrWb6(DSAwUA2_ zMHKkwe5Mt`->z`Z#nV%Tb9VlCx57CqwW2N89=i+Y zWpzp=%%v!tbIFvpLPVXwlL$X5h4Z(Y72ct6KJqej)=*DSe8cs1S;rj@RPgvF>+2Y% zvg&p2gt0cveP1;O{Jr({953{bYQ1XO@w!B!t!4@KSr@GD`-K$*J z3R47up_0X}ZoVYCy9dFOr*y!1ys+8%__K7&S$l$BckvW_Ww) zc?IqP>fy2W0IhnFD_2{4DW<9%^_Ui=JceQ*D*<~bR#xp4XvmY5B}MD5&K2YLC<}H* zfwGwq&nPn$Q}Ae5VDcpgxO)5&pbpe|SBPdyZTcJy)JTn$!!%O+?-(9*zjAqhIQ@6L zQ(%JB8j3q$JkD?%hfNQl&A762eS*SKAl$NvT_UGZ&kTh(W&}0GVr01IR9S@=%VB9` zSfGx_mg#7v9zQZsdH*-CgU3cr@oT^?cjXVx=wm|hX(0f7%&AXbWvSfDHS@IMH!b|8 zg`9JJ)57ldO$!^59R|V;oKZCD`5S4WeU80UxNu+y1nPKfnT}TKZ(8`4z%FaX`wyz1!Q|`)FT4a@z^pQt<>MN z@GXH|?m8wdWRAoL!k}rPLYDF04hkyb7cXpw+ueQ{p8J9m`2v3Y&i!?X$NpdKJdcMR gW;z#v^=7&H4;~>Hp|qzV761SM07*qoM6N<$f)uY=6#xJL 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..25149d251c --- /dev/null +++ b/mrs_uav_trajectory_generation/src/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)2
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)4
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::transformPath(mrs_msgs::Path_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, double const&, bool const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&)6
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::prepareInitialCondition(ros::Time)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, bool, bool)11
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()16
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::validateTrajectorySpatial(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)17
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)24
(anonymous namespace)::ProxyExec0::ProxyExec0()107
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)107
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()107
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)898
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::distFromSegment(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)5142
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()9776
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)170092
+
+
+ + + +
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..f2d7dfe67c --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:747109568.2 %
Date:2024-06-08 22:13:12Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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