From c5b6dfa26cab7938c0e5fadb8198212da1937b07 Mon Sep 17 00:00:00 2001 From: klaxalk Date: Sun, 7 Jan 2024 21:47:19 +0000 Subject: [PATCH] deploy: 2c32258ccb988205b9c7884efd1c31a4dd707ed3 --- .nojekyll | 0 amber.png | Bin 0 -> 141 bytes badge.svg | 1 + emerald.png | Bin 0 -> 141 bytes gcov.css | 519 + glass.png | Bin 0 -> 167 bytes index-sort-f.html | 662 ++ index-sort-l.html | 662 ++ index.html | 662 ++ .../attitude_converter.h.func-sort-c.html | 128 + .../mrs_lib/attitude_converter.h.func.html | 128 + .../attitude_converter.h.gcov.frameset.html | 19 + .../mrs_lib/attitude_converter.h.gcov.html | 617 ++ .../attitude_converter.h.gcov.overview.html | 154 + .../mrs_lib/attitude_converter.h.gcov.png | Bin 0 -> 1660 bytes ...dynamic_reconfigure_mgr.h.func-sort-c.html | 420 + .../dynamic_reconfigure_mgr.h.func.html | 420 + ...namic_reconfigure_mgr.h.gcov.frameset.html | 19 + .../dynamic_reconfigure_mgr.h.gcov.html | 343 + ...namic_reconfigure_mgr.h.gcov.overview.html | 85 + .../dynamic_reconfigure_mgr.h.gcov.png | Bin 0 -> 1370 bytes .../geometry/cyclic.h.func-sort-c.html | 548 + .../mrs_lib/geometry/cyclic.h.func.html | 548 + .../geometry/cyclic.h.gcov.frameset.html | 19 + .../mrs_lib/geometry/cyclic.h.gcov.html | 612 ++ .../geometry/cyclic.h.gcov.overview.html | 152 + .../mrs_lib/geometry/cyclic.h.gcov.png | Bin 0 -> 2145 bytes .../mrs_lib/geometry/index-detail-sort-f.html | 128 + .../mrs_lib/geometry/index-detail-sort-l.html | 128 + .../mrs_lib/geometry/index-detail.html | 128 + .../mrs_lib/geometry/index-sort-f.html | 112 + .../mrs_lib/geometry/index-sort-l.html | 112 + mrs_lib/include/mrs_lib/geometry/index.html | 112 + .../mrs_lib/geometry/misc.h.func-sort-c.html | 92 + .../include/mrs_lib/geometry/misc.h.func.html | 92 + .../geometry/misc.h.gcov.frameset.html | 19 + .../include/mrs_lib/geometry/misc.h.gcov.html | 408 + .../geometry/misc.h.gcov.overview.html | 101 + .../include/mrs_lib/geometry/misc.h.gcov.png | Bin 0 -> 1071 bytes .../gps_conversions.h.func-sort-c.html | 96 + .../mrs_lib/gps_conversions.h.func.html | 96 + .../gps_conversions.h.gcov.frameset.html | 19 + .../mrs_lib/gps_conversions.h.gcov.html | 387 + .../gps_conversions.h.gcov.overview.html | 96 + .../mrs_lib/gps_conversions.h.gcov.png | Bin 0 -> 1579 bytes .../mrs_lib/impl/index-detail-sort-f.html | 218 + .../mrs_lib/impl/index-detail-sort-l.html | 218 + .../include/mrs_lib/impl/index-detail.html | 218 + .../include/mrs_lib/impl/index-sort-f.html | 162 + .../include/mrs_lib/impl/index-sort-l.html | 162 + mrs_lib/include/mrs_lib/impl/index.html | 162 + .../impl/param_provider.hpp.func-sort-c.html | 144 + .../mrs_lib/impl/param_provider.hpp.func.html | 144 + .../param_provider.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/param_provider.hpp.gcov.html | 132 + .../param_provider.hpp.gcov.overview.html | 32 + .../mrs_lib/impl/param_provider.hpp.gcov.png | Bin 0 -> 347 bytes .../publisher_handler.hpp.func-sort-c.html | 876 ++ .../impl/publisher_handler.hpp.func.html | 876 ++ .../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 -> 799 bytes ...ervice_client_handler.hpp.func-sort-c.html | 228 + .../impl/service_client_handler.hpp.func.html | 228 + ...vice_client_handler.hpp.gcov.frameset.html | 19 + .../impl/service_client_handler.hpp.gcov.html | 403 + ...vice_client_handler.hpp.gcov.overview.html | 100 + .../impl/service_client_handler.hpp.gcov.png | Bin 0 -> 969 bytes .../subscribe_handler.hpp.func-sort-c.html | 4080 ++++++++ .../impl/subscribe_handler.hpp.func.html | 4080 ++++++++ .../subscribe_handler.hpp.gcov.frameset.html | 19 + .../impl/subscribe_handler.hpp.gcov.html | 413 + .../subscribe_handler.hpp.gcov.overview.html | 103 + .../impl/subscribe_handler.hpp.gcov.png | Bin 0 -> 1386 bytes .../mrs_lib/impl/timer.hpp.func-sort-c.html | 92 + .../include/mrs_lib/impl/timer.hpp.func.html | 92 + .../mrs_lib/impl/timer.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/timer.hpp.gcov.html | 179 + .../mrs_lib/impl/timer.hpp.gcov.overview.html | 44 + .../include/mrs_lib/impl/timer.hpp.gcov.png | Bin 0 -> 527 bytes .../impl/transformer.hpp.func-sort-c.html | 272 + .../mrs_lib/impl/transformer.hpp.func.html | 272 + .../impl/transformer.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/transformer.hpp.gcov.html | 280 + .../impl/transformer.hpp.gcov.overview.html | 69 + .../mrs_lib/impl/transformer.hpp.gcov.png | Bin 0 -> 841 bytes .../mrs_lib/impl/ukf.hpp.func-sort-c.html | 128 + .../include/mrs_lib/impl/ukf.hpp.func.html | 128 + .../mrs_lib/impl/ukf.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/ukf.hpp.gcov.html | 364 + .../mrs_lib/impl/ukf.hpp.gcov.overview.html | 90 + mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png | Bin 0 -> 1198 bytes .../include/mrs_lib/index-detail-sort-f.html | 426 + .../include/mrs_lib/index-detail-sort-l.html | 426 + mrs_lib/include/mrs_lib/index-detail.html | 426 + mrs_lib/include/mrs_lib/index-sort-f.html | 282 + mrs_lib/include/mrs_lib/index-sort-l.html | 282 + mrs_lib/include/mrs_lib/index.html | 282 + .../include/mrs_lib/lkf.h.func-sort-c.html | 244 + mrs_lib/include/mrs_lib/lkf.h.func.html | 244 + .../include/mrs_lib/lkf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/lkf.h.gcov.html | 460 + .../include/mrs_lib/lkf.h.gcov.overview.html | 114 + mrs_lib/include/mrs_lib/lkf.h.gcov.png | Bin 0 -> 1750 bytes .../mrs_lib/msg_extractor.h.func-sort-c.html | 256 + .../include/mrs_lib/msg_extractor.h.func.html | 256 + .../msg_extractor.h.gcov.frameset.html | 19 + .../include/mrs_lib/msg_extractor.h.gcov.html | 775 ++ .../msg_extractor.h.gcov.overview.html | 193 + .../include/mrs_lib/msg_extractor.h.gcov.png | Bin 0 -> 1287 bytes .../include/mrs_lib/mutex.h.func-sort-c.html | 404 + mrs_lib/include/mrs_lib/mutex.h.func.html | 404 + .../mrs_lib/mutex.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/mutex.h.gcov.html | 260 + .../mrs_lib/mutex.h.gcov.overview.html | 64 + mrs_lib/include/mrs_lib/mutex.h.gcov.png | Bin 0 -> 739 bytes .../mrs_lib/param_loader.h.func-sort-c.html | 368 + .../include/mrs_lib/param_loader.h.func.html | 368 + .../mrs_lib/param_loader.h.gcov.frameset.html | 19 + .../include/mrs_lib/param_loader.h.gcov.html | 1397 +++ .../mrs_lib/param_loader.h.gcov.overview.html | 349 + .../include/mrs_lib/param_loader.h.gcov.png | Bin 0 -> 4285 bytes .../publisher_handler.h.func-sort-c.html | 548 + .../mrs_lib/publisher_handler.h.func.html | 548 + .../publisher_handler.h.gcov.frameset.html | 19 + .../mrs_lib/publisher_handler.h.gcov.html | 256 + .../publisher_handler.h.gcov.overview.html | 63 + .../mrs_lib/publisher_handler.h.gcov.png | Bin 0 -> 572 bytes ...uadratic_throttle_model.h.func-sort-c.html | 88 + .../quadratic_throttle_model.h.func.html | 88 + ...dratic_throttle_model.h.gcov.frameset.html | 19 + .../quadratic_throttle_model.h.gcov.html | 117 + ...dratic_throttle_model.h.gcov.overview.html | 29 + .../quadratic_throttle_model.h.gcov.png | Bin 0 -> 246 bytes .../mrs_lib/repredictor.h.func-sort-c.html | 304 + .../include/mrs_lib/repredictor.h.func.html | 304 + .../mrs_lib/repredictor.h.gcov.frameset.html | 19 + .../include/mrs_lib/repredictor.h.gcov.html | 637 ++ .../mrs_lib/repredictor.h.gcov.overview.html | 159 + .../include/mrs_lib/repredictor.h.gcov.png | Bin 0 -> 2387 bytes .../safety_zone/index-detail-sort-f.html | 112 + .../safety_zone/index-detail-sort-l.html | 112 + .../mrs_lib/safety_zone/index-detail.html | 112 + .../mrs_lib/safety_zone/index-sort-f.html | 112 + .../mrs_lib/safety_zone/index-sort-l.html | 112 + .../include/mrs_lib/safety_zone/index.html | 112 + .../safety_zone/polygon.h.func-sort-c.html | 92 + .../mrs_lib/safety_zone/polygon.h.func.html | 92 + .../safety_zone/polygon.h.gcov.frameset.html | 19 + .../mrs_lib/safety_zone/polygon.h.gcov.html | 136 + .../safety_zone/polygon.h.gcov.overview.html | 33 + .../mrs_lib/safety_zone/polygon.h.gcov.png | Bin 0 -> 337 bytes .../safety_zone.h.func-sort-c.html | 84 + .../safety_zone/safety_zone.h.func.html | 84 + .../safety_zone.h.gcov.frameset.html | 19 + .../safety_zone/safety_zone.h.gcov.html | 121 + .../safety_zone.h.gcov.overview.html | 30 + .../safety_zone/safety_zone.h.gcov.png | Bin 0 -> 280 bytes .../mrs_lib/scope_timer.h.func-sort-c.html | 92 + .../include/mrs_lib/scope_timer.h.func.html | 92 + .../mrs_lib/scope_timer.h.gcov.frameset.html | 19 + .../include/mrs_lib/scope_timer.h.gcov.html | 248 + .../mrs_lib/scope_timer.h.gcov.overview.html | 61 + .../include/mrs_lib/scope_timer.h.gcov.png | Bin 0 -> 777 bytes .../service_client_handler.h.func-sort-c.html | 160 + .../service_client_handler.h.func.html | 160 + ...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 | 2916 ++++++ .../mrs_lib/subscribe_handler.h.func.html | 2916 ++++++ .../subscribe_handler.h.gcov.frameset.html | 19 + .../mrs_lib/subscribe_handler.h.gcov.html | 548 + .../subscribe_handler.h.gcov.overview.html | 136 + .../mrs_lib/subscribe_handler.h.gcov.png | Bin 0 -> 1794 bytes .../timeout_manager.h.func-sort-c.html | 80 + .../mrs_lib/timeout_manager.h.func.html | 80 + .../timeout_manager.h.gcov.frameset.html | 19 + .../mrs_lib/timeout_manager.h.gcov.html | 169 + .../timeout_manager.h.gcov.overview.html | 42 + .../mrs_lib/timeout_manager.h.gcov.png | Bin 0 -> 435 bytes .../include/mrs_lib/timer.h.func-sort-c.html | 108 + mrs_lib/include/mrs_lib/timer.h.func.html | 108 + .../mrs_lib/timer.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/timer.h.gcov.html | 328 + .../mrs_lib/timer.h.gcov.overview.html | 81 + mrs_lib/include/mrs_lib/timer.h.gcov.png | Bin 0 -> 896 bytes .../mrs_lib/transformer.h.func-sort-c.html | 152 + .../include/mrs_lib/transformer.h.func.html | 152 + .../mrs_lib/transformer.h.gcov.frameset.html | 19 + .../include/mrs_lib/transformer.h.gcov.html | 764 ++ .../mrs_lib/transformer.h.gcov.overview.html | 190 + .../include/mrs_lib/transformer.h.gcov.png | Bin 0 -> 2328 bytes .../include/mrs_lib/ukf.h.func-sort-c.html | 88 + mrs_lib/include/mrs_lib/ukf.h.func.html | 88 + .../include/mrs_lib/ukf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/ukf.h.gcov.html | 285 + .../include/mrs_lib/ukf.h.gcov.overview.html | 71 + mrs_lib/include/mrs_lib/ukf.h.gcov.png | Bin 0 -> 1009 bytes .../include/mrs_lib/utils.h.func-sort-c.html | 100 + mrs_lib/include/mrs_lib/utils.h.func.html | 100 + .../mrs_lib/utils.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/utils.h.gcov.html | 229 + .../mrs_lib/utils.h.gcov.overview.html | 57 + mrs_lib/include/mrs_lib/utils.h.gcov.png | Bin 0 -> 677 bytes .../mrs_lib/visual_object.h.func-sort-c.html | 84 + .../include/mrs_lib/visual_object.h.func.html | 84 + .../visual_object.h.gcov.frameset.html | 19 + .../include/mrs_lib/visual_object.h.gcov.html | 182 + .../visual_object.h.gcov.overview.html | 45 + .../include/mrs_lib/visual_object.h.gcov.png | Bin 0 -> 484 bytes .../attitude_converter.cpp.func-sort-c.html | 244 + .../attitude_converter.cpp.func.html | 244 + .../attitude_converter.cpp.gcov.frameset.html | 19 + .../attitude_converter.cpp.gcov.html | 561 ++ .../attitude_converter.cpp.gcov.overview.html | 140 + .../attitude_converter.cpp.gcov.png | Bin 0 -> 1533 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../src/attitude_converter/index-detail.html | 110 + .../src/attitude_converter/index-sort-f.html | 102 + .../src/attitude_converter/index-sort-l.html | 102 + mrs_lib/src/attitude_converter/index.html | 102 + .../batch_visualizer.cpp.func-sort-c.html | 168 + .../batch_visualizer.cpp.func.html | 168 + .../batch_visualizer.cpp.gcov.frameset.html | 19 + .../batch_visualizer.cpp.gcov.html | 432 + .../batch_visualizer.cpp.gcov.overview.html | 107 + .../batch_visualizer.cpp.gcov.png | Bin 0 -> 1246 bytes .../batch_visualizer/index-detail-sort-f.html | 128 + .../batch_visualizer/index-detail-sort-l.html | 128 + .../src/batch_visualizer/index-detail.html | 128 + .../src/batch_visualizer/index-sort-f.html | 112 + .../src/batch_visualizer/index-sort-l.html | 112 + mrs_lib/src/batch_visualizer/index.html | 112 + .../visual_object.cpp.func-sort-c.html | 164 + .../visual_object.cpp.func.html | 164 + .../visual_object.cpp.gcov.frameset.html | 19 + .../visual_object.cpp.gcov.html | 401 + .../visual_object.cpp.gcov.overview.html | 100 + .../visual_object.cpp.gcov.png | Bin 0 -> 1166 bytes .../geometry/conversions.cpp.func-sort-c.html | 120 + .../src/geometry/conversions.cpp.func.html | 120 + .../conversions.cpp.gcov.frameset.html | 19 + .../src/geometry/conversions.cpp.gcov.html | 178 + .../conversions.cpp.gcov.overview.html | 44 + mrs_lib/src/geometry/conversions.cpp.gcov.png | Bin 0 -> 446 bytes mrs_lib/src/geometry/index-detail-sort-f.html | 138 + mrs_lib/src/geometry/index-detail-sort-l.html | 138 + mrs_lib/src/geometry/index-detail.html | 138 + mrs_lib/src/geometry/index-sort-f.html | 122 + mrs_lib/src/geometry/index-sort-l.html | 122 + mrs_lib/src/geometry/index.html | 122 + .../src/geometry/misc.cpp.func-sort-c.html | 144 + mrs_lib/src/geometry/misc.cpp.func.html | 144 + .../src/geometry/misc.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/misc.cpp.gcov.html | 269 + .../src/geometry/misc.cpp.gcov.overview.html | 67 + mrs_lib/src/geometry/misc.cpp.gcov.png | Bin 0 -> 760 bytes .../src/geometry/shapes.cpp.func-sort-c.html | 352 + mrs_lib/src/geometry/shapes.cpp.func.html | 352 + .../geometry/shapes.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/shapes.cpp.gcov.html | 730 ++ .../geometry/shapes.cpp.gcov.overview.html | 182 + mrs_lib/src/geometry/shapes.cpp.gcov.png | Bin 0 -> 1596 bytes mrs_lib/src/math/index-detail-sort-f.html | 110 + mrs_lib/src/math/index-detail-sort-l.html | 110 + mrs_lib/src/math/index-detail.html | 110 + mrs_lib/src/math/index-sort-f.html | 102 + mrs_lib/src/math/index-sort-l.html | 102 + mrs_lib/src/math/index.html | 102 + mrs_lib/src/math/math.cpp.func-sort-c.html | 84 + mrs_lib/src/math/math.cpp.func.html | 84 + mrs_lib/src/math/math.cpp.gcov.frameset.html | 19 + mrs_lib/src/math/math.cpp.gcov.html | 149 + mrs_lib/src/math/math.cpp.gcov.overview.html | 37 + mrs_lib/src/math/math.cpp.gcov.png | Bin 0 -> 388 bytes .../median_filter/index-detail-sort-f.html | 110 + .../median_filter/index-detail-sort-l.html | 110 + mrs_lib/src/median_filter/index-detail.html | 110 + mrs_lib/src/median_filter/index-sort-f.html | 102 + mrs_lib/src/median_filter/index-sort-l.html | 102 + mrs_lib/src/median_filter/index.html | 102 + .../median_filter.cpp.func-sort-c.html | 148 + .../median_filter/median_filter.cpp.func.html | 148 + .../median_filter.cpp.gcov.frameset.html | 19 + .../median_filter/median_filter.cpp.gcov.html | 286 + .../median_filter.cpp.gcov.overview.html | 71 + .../median_filter/median_filter.cpp.gcov.png | Bin 0 -> 872 bytes .../src/param_loader/index-detail-sort-f.html | 110 + .../src/param_loader/index-detail-sort-l.html | 110 + mrs_lib/src/param_loader/index-detail.html | 110 + mrs_lib/src/param_loader/index-sort-f.html | 102 + mrs_lib/src/param_loader/index-sort-l.html | 102 + mrs_lib/src/param_loader/index.html | 102 + .../param_provider.cpp.func-sort-c.html | 96 + .../param_loader/param_provider.cpp.func.html | 96 + .../param_provider.cpp.gcov.frameset.html | 19 + .../param_loader/param_provider.cpp.gcov.html | 200 + .../param_provider.cpp.gcov.overview.html | 49 + .../param_loader/param_provider.cpp.gcov.png | Bin 0 -> 659 bytes mrs_lib/src/profiler/index-detail-sort-f.html | 110 + mrs_lib/src/profiler/index-detail-sort-l.html | 110 + mrs_lib/src/profiler/index-detail.html | 110 + mrs_lib/src/profiler/index-sort-f.html | 102 + mrs_lib/src/profiler/index-sort-l.html | 102 + mrs_lib/src/profiler/index.html | 102 + .../profiler/profiler.cpp.func-sort-c.html | 120 + mrs_lib/src/profiler/profiler.cpp.func.html | 120 + .../profiler/profiler.cpp.gcov.frameset.html | 19 + mrs_lib/src/profiler/profiler.cpp.gcov.html | 301 + .../profiler/profiler.cpp.gcov.overview.html | 75 + mrs_lib/src/profiler/profiler.cpp.gcov.png | Bin 0 -> 841 bytes .../src/safety_zone/index-detail-sort-f.html | 128 + .../src/safety_zone/index-detail-sort-l.html | 128 + mrs_lib/src/safety_zone/index-detail.html | 128 + mrs_lib/src/safety_zone/index-sort-f.html | 112 + mrs_lib/src/safety_zone/index-sort-l.html | 112 + mrs_lib/src/safety_zone/index.html | 112 + .../line_operations.cpp.func-sort-c.html | 92 + .../safety_zone/line_operations.cpp.func.html | 92 + .../line_operations.cpp.gcov.frameset.html | 19 + .../safety_zone/line_operations.cpp.gcov.html | 143 + .../line_operations.cpp.gcov.overview.html | 35 + .../safety_zone/line_operations.cpp.gcov.png | Bin 0 -> 465 bytes .../polygon/index-detail-sort-f.html | 110 + .../polygon/index-detail-sort-l.html | 110 + .../src/safety_zone/polygon/index-detail.html | 110 + .../src/safety_zone/polygon/index-sort-f.html | 102 + .../src/safety_zone/polygon/index-sort-l.html | 102 + mrs_lib/src/safety_zone/polygon/index.html | 102 + .../polygon/polygon.cpp.func-sort-c.html | 112 + .../safety_zone/polygon/polygon.cpp.func.html | 112 + .../polygon/polygon.cpp.gcov.frameset.html | 19 + .../safety_zone/polygon/polygon.cpp.gcov.html | 290 + .../polygon/polygon.cpp.gcov.overview.html | 72 + .../safety_zone/polygon/polygon.cpp.gcov.png | Bin 0 -> 1057 bytes .../safety_zone.cpp.func-sort-c.html | 104 + .../src/safety_zone/safety_zone.cpp.func.html | 104 + .../safety_zone.cpp.gcov.frameset.html | 19 + .../src/safety_zone/safety_zone.cpp.gcov.html | 160 + .../safety_zone.cpp.gcov.overview.html | 39 + .../src/safety_zone/safety_zone.cpp.gcov.png | Bin 0 -> 380 bytes .../src/scope_timer/index-detail-sort-f.html | 110 + .../src/scope_timer/index-detail-sort-l.html | 110 + mrs_lib/src/scope_timer/index-detail.html | 110 + mrs_lib/src/scope_timer/index-sort-f.html | 102 + mrs_lib/src/scope_timer/index-sort-l.html | 102 + mrs_lib/src/scope_timer/index.html | 102 + .../scope_timer.cpp.func-sort-c.html | 120 + .../src/scope_timer/scope_timer.cpp.func.html | 120 + .../scope_timer.cpp.gcov.frameset.html | 19 + .../src/scope_timer/scope_timer.cpp.gcov.html | 345 + .../scope_timer.cpp.gcov.overview.html | 86 + .../src/scope_timer/scope_timer.cpp.gcov.png | Bin 0 -> 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 | 186 + .../timeout_manager.cpp.gcov.overview.html | 46 + .../timeout_manager.cpp.gcov.png | Bin 0 -> 512 bytes mrs_lib/src/timer/index-detail-sort-f.html | 110 + mrs_lib/src/timer/index-detail-sort-l.html | 110 + mrs_lib/src/timer/index-detail.html | 110 + mrs_lib/src/timer/index-sort-f.html | 102 + mrs_lib/src/timer/index-sort-l.html | 102 + mrs_lib/src/timer/index.html | 102 + mrs_lib/src/timer/timer.cpp.func-sort-c.html | 140 + mrs_lib/src/timer/timer.cpp.func.html | 140 + .../src/timer/timer.cpp.gcov.frameset.html | 19 + mrs_lib/src/timer/timer.cpp.gcov.html | 339 + .../src/timer/timer.cpp.gcov.overview.html | 84 + mrs_lib/src/timer/timer.cpp.gcov.png | Bin 0 -> 1067 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../transform_broadcaster/index-detail.html | 110 + .../transform_broadcaster/index-sort-f.html | 102 + .../transform_broadcaster/index-sort-l.html | 102 + mrs_lib/src/transform_broadcaster/index.html | 102 + ...transform_broadcaster.cpp.func-sort-c.html | 92 + .../transform_broadcaster.cpp.func.html | 92 + ...ansform_broadcaster.cpp.gcov.frameset.html | 19 + .../transform_broadcaster.cpp.gcov.html | 140 + ...ansform_broadcaster.cpp.gcov.overview.html | 34 + .../transform_broadcaster.cpp.gcov.png | Bin 0 -> 425 bytes .../src/transformer/index-detail-sort-f.html | 110 + .../src/transformer/index-detail-sort-l.html | 110 + mrs_lib/src/transformer/index-detail.html | 110 + mrs_lib/src/transformer/index-sort-f.html | 102 + mrs_lib/src/transformer/index-sort-l.html | 102 + mrs_lib/src/transformer/index.html | 102 + .../transformer.cpp.func-sort-c.html | 184 + .../src/transformer/transformer.cpp.func.html | 184 + .../transformer.cpp.gcov.frameset.html | 19 + .../src/transformer/transformer.cpp.gcov.html | 671 ++ .../transformer.cpp.gcov.overview.html | 167 + .../src/transformer/transformer.cpp.gcov.png | Bin 0 -> 1995 bytes mrs_lib/src/utils/index-detail-sort-f.html | 110 + mrs_lib/src/utils/index-detail-sort-l.html | 110 + mrs_lib/src/utils/index-detail.html | 110 + mrs_lib/src/utils/index-sort-f.html | 102 + mrs_lib/src/utils/index-sort-l.html | 102 + mrs_lib/src/utils/index.html | 102 + mrs_lib/src/utils/utils.cpp.func-sort-c.html | 88 + mrs_lib/src/utils/utils.cpp.func.html | 88 + .../src/utils/utils.cpp.gcov.frameset.html | 19 + mrs_lib/src/utils/utils.cpp.gcov.html | 101 + .../src/utils/utils.cpp.gcov.overview.html | 25 + mrs_lib/src/utils/utils.cpp.gcov.png | Bin 0 -> 194 bytes .../src/automatic_start.cpp.func-sort-c.html | 156 + .../src/automatic_start.cpp.func.html | 156 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 887 ++ .../automatic_start.cpp.gcov.overview.html | 221 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 2658 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + mrs_uav_autostart/src/index-detail.html | 110 + mrs_uav_autostart/src/index-sort-f.html | 102 + mrs_uav_autostart/src/index-sort-l.html | 102 + mrs_uav_autostart/src/index.html | 102 + .../include/common.h.func-sort-c.html | 116 + .../include/common.h.func.html | 116 + .../include/common.h.gcov.frameset.html | 19 + .../include/common.h.gcov.html | 189 + .../include/common.h.gcov.overview.html | 47 + mrs_uav_controllers/include/common.h.gcov.png | Bin 0 -> 436 bytes .../include/index-detail-sort-f.html | 128 + .../include/index-detail-sort-l.html | 128 + mrs_uav_controllers/include/index-detail.html | 128 + mrs_uav_controllers/include/index-sort-f.html | 112 + mrs_uav_controllers/include/index-sort-l.html | 112 + mrs_uav_controllers/include/index.html | 112 + .../include/pid.hpp.func-sort-c.html | 100 + mrs_uav_controllers/include/pid.hpp.func.html | 100 + .../include/pid.hpp.gcov.frameset.html | 19 + mrs_uav_controllers/include/pid.hpp.gcov.html | 184 + .../include/pid.hpp.gcov.overview.html | 45 + mrs_uav_controllers/include/pid.hpp.gcov.png | Bin 0 -> 533 bytes .../src/common.cpp.func-sort-c.html | 116 + mrs_uav_controllers/src/common.cpp.func.html | 116 + .../src/common.cpp.gcov.frameset.html | 19 + mrs_uav_controllers/src/common.cpp.gcov.html | 471 + .../src/common.cpp.gcov.overview.html | 117 + mrs_uav_controllers/src/common.cpp.gcov.png | Bin 0 -> 1366 bytes .../failsafe_controller.cpp.func-sort-c.html | 124 + .../src/failsafe_controller.cpp.func.html | 124 + ...failsafe_controller.cpp.gcov.frameset.html | 19 + .../src/failsafe_controller.cpp.gcov.html | 635 ++ ...failsafe_controller.cpp.gcov.overview.html | 158 + .../src/failsafe_controller.cpp.gcov.png | Bin 0 -> 1814 bytes .../src/index-detail-sort-f.html | 182 + .../src/index-detail-sort-l.html | 182 + mrs_uav_controllers/src/index-detail.html | 182 + mrs_uav_controllers/src/index-sort-f.html | 142 + mrs_uav_controllers/src/index-sort-l.html | 142 + mrs_uav_controllers/src/index.html | 142 + ...activation_controller.cpp.func-sort-c.html | 124 + ...midair_activation_controller.cpp.func.html | 124 + ...tivation_controller.cpp.gcov.frameset.html | 19 + ...midair_activation_controller.cpp.gcov.html | 518 + ...tivation_controller.cpp.gcov.overview.html | 129 + .../midair_activation_controller.cpp.gcov.png | Bin 0 -> 1228 bytes .../src/mpc_controller.cpp.func-sort-c.html | 152 + .../src/mpc_controller.cpp.func.html | 152 + .../src/mpc_controller.cpp.gcov.frameset.html | 19 + .../src/mpc_controller.cpp.gcov.html | 2214 +++++ .../src/mpc_controller.cpp.gcov.overview.html | 553 ++ .../src/mpc_controller.cpp.gcov.png | Bin 0 -> 6648 bytes .../src/se3_controller.cpp.func-sort-c.html | 148 + .../src/se3_controller.cpp.func.html | 148 + .../src/se3_controller.cpp.gcov.frameset.html | 19 + .../src/se3_controller.cpp.gcov.html | 1927 ++++ .../src/se3_controller.cpp.gcov.overview.html | 481 + .../src/se3_controller.cpp.gcov.png | Bin 0 -> 5825 bytes .../control_manager/index-detail-sort-f.html | 110 + .../control_manager/index-detail-sort-l.html | 110 + .../include/control_manager/index-detail.html | 110 + .../include/control_manager/index-sort-f.html | 102 + .../include/control_manager/index-sort-l.html | 102 + .../include/control_manager/index.html | 102 + .../output_publisher.h.func-sort-c.html | 120 + .../output_publisher.h.func.html | 120 + .../output_publisher.h.gcov.frameset.html | 19 + .../output_publisher.h.gcov.html | 146 + .../output_publisher.h.gcov.overview.html | 36 + .../output_publisher.h.gcov.png | Bin 0 -> 332 bytes .../agl_estimator.h.func-sort-c.html | 92 + .../agl_estimator.h.func.html | 92 + .../agl_estimator.h.gcov.frameset.html | 19 + .../agl_estimator.h.gcov.html | 157 + .../agl_estimator.h.gcov.overview.html | 39 + .../mrs_uav_managers/agl_estimator.h.gcov.png | Bin 0 -> 397 bytes .../control_manager/common.h.func-sort-c.html | 188 + .../control_manager/common.h.func.html | 188 + .../common.h.gcov.frameset.html | 19 + .../control_manager/common.h.gcov.html | 380 + .../common.h.gcov.overview.html | 94 + .../control_manager/common.h.gcov.png | Bin 0 -> 934 bytes .../control_manager/index-detail-sort-f.html | 110 + .../control_manager/index-detail-sort-l.html | 110 + .../control_manager/index-detail.html | 110 + .../control_manager/index-sort-f.html | 102 + .../control_manager/index-sort-l.html | 102 + .../control_manager/index.html | 102 + .../controller.h.func-sort-c.html | 88 + .../mrs_uav_managers/controller.h.func.html | 88 + .../controller.h.gcov.frameset.html | 19 + .../mrs_uav_managers/controller.h.gcov.html | 232 + .../controller.h.gcov.overview.html | 57 + .../mrs_uav_managers/controller.h.gcov.png | Bin 0 -> 681 bytes .../estimator.h.func-sort-c.html | 92 + .../estimation_manager/estimator.h.func.html | 92 + .../estimator.h.gcov.frameset.html | 19 + .../estimation_manager/estimator.h.gcov.html | 195 + .../estimator.h.gcov.overview.html | 48 + .../estimation_manager/estimator.h.gcov.png | Bin 0 -> 505 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../estimation_manager/index-detail.html | 128 + .../estimation_manager/index-sort-f.html | 112 + .../estimation_manager/index-sort-l.html | 112 + .../estimation_manager/index.html | 112 + .../support.h.func-sort-c.html | 140 + .../estimation_manager/support.h.func.html | 140 + .../support.h.gcov.frameset.html | 19 + .../estimation_manager/support.h.gcov.html | 405 + .../support.h.gcov.overview.html | 101 + .../estimation_manager/support.h.gcov.png | Bin 0 -> 1232 bytes .../mrs_uav_managers/index-detail-sort-f.html | 164 + .../mrs_uav_managers/index-detail-sort-l.html | 164 + .../mrs_uav_managers/index-detail.html | 164 + .../mrs_uav_managers/index-sort-f.html | 132 + .../mrs_uav_managers/index-sort-l.html | 132 + .../include/mrs_uav_managers/index.html | 132 + .../state_estimator.h.func-sort-c.html | 92 + .../state_estimator.h.func.html | 92 + .../state_estimator.h.gcov.frameset.html | 19 + .../state_estimator.h.gcov.html | 169 + .../state_estimator.h.gcov.overview.html | 42 + .../state_estimator.h.gcov.png | Bin 0 -> 435 bytes .../tracker.h.func-sort-c.html | 88 + .../mrs_uav_managers/tracker.h.func.html | 88 + .../tracker.h.gcov.frameset.html | 19 + .../mrs_uav_managers/tracker.h.gcov.html | 296 + .../tracker.h.gcov.overview.html | 73 + .../mrs_uav_managers/tracker.h.gcov.png | Bin 0 -> 751 bytes .../index-detail-sort-f.html | 120 + .../index-detail-sort-l.html | 120 + .../transform_manager/index-detail.html | 120 + .../transform_manager/index-sort-f.html | 112 + .../transform_manager/index-sort-l.html | 112 + .../include/transform_manager/index.html | 112 + .../tf_mapping_origin.h.func-sort-c.html | 100 + .../tf_mapping_origin.h.func.html | 100 + .../tf_mapping_origin.h.gcov.frameset.html | 19 + .../tf_mapping_origin.h.gcov.html | 474 + .../tf_mapping_origin.h.gcov.overview.html | 118 + .../tf_mapping_origin.h.gcov.png | Bin 0 -> 1536 bytes .../tf_source.h.func-sort-c.html | 136 + .../transform_manager/tf_source.h.func.html | 136 + .../tf_source.h.gcov.frameset.html | 19 + .../transform_manager/tf_source.h.gcov.html | 704 ++ .../tf_source.h.gcov.overview.html | 175 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2161 bytes .../constraint_manager.cpp.func-sort-c.html | 112 + .../src/constraint_manager.cpp.func.html | 112 + .../constraint_manager.cpp.gcov.frameset.html | 19 + .../src/constraint_manager.cpp.gcov.html | 706 ++ .../constraint_manager.cpp.gcov.overview.html | 176 + .../src/constraint_manager.cpp.gcov.png | Bin 0 -> 2164 bytes .../common/common.cpp.func-sort-c.html | 204 + .../common/common.cpp.func.html | 204 + .../common/common.cpp.gcov.frameset.html | 19 + .../common/common.cpp.gcov.html | 1301 +++ .../common/common.cpp.gcov.overview.html | 325 + .../common/common.cpp.gcov.png | Bin 0 -> 2398 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 | 520 + .../control_manager.cpp.func.html | 520 + .../control_manager.cpp.gcov.frameset.html | 19 + .../control_manager.cpp.gcov.html | 8807 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2201 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 25810 bytes .../control_manager/index-detail-sort-f.html | 128 + .../control_manager/index-detail-sort-l.html | 128 + .../src/control_manager/index-detail.html | 128 + .../src/control_manager/index-sort-f.html | 112 + .../src/control_manager/index-sort-l.html | 112 + .../src/control_manager/index.html | 112 + .../output_publisher.cpp.func-sort-c.html | 128 + .../output_publisher.cpp.func.html | 128 + .../output_publisher.cpp.gcov.frameset.html | 19 + .../output_publisher.cpp.gcov.html | 154 + .../output_publisher.cpp.gcov.overview.html | 38 + .../output_publisher.cpp.gcov.png | Bin 0 -> 307 bytes .../estimation_manager.cpp.func-sort-c.html | 176 + .../estimation_manager.cpp.func.html | 176 + .../estimation_manager.cpp.gcov.frameset.html | 19 + .../estimation_manager.cpp.gcov.html | 1313 +++ .../estimation_manager.cpp.gcov.overview.html | 328 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4278 bytes .../estimator.cpp.func-sort-c.html | 172 + .../estimator.cpp.func.html | 172 + .../estimator.cpp.gcov.frameset.html | 19 + .../estimator.cpp.gcov.html | 290 + .../estimator.cpp.gcov.overview.html | 72 + .../estimation_manager/estimator.cpp.gcov.png | Bin 0 -> 799 bytes .../agl_estimator.cpp.func-sort-c.html | 92 + .../estimators/agl_estimator.cpp.func.html | 92 + .../agl_estimator.cpp.gcov.frameset.html | 19 + .../estimators/agl_estimator.cpp.gcov.html | 182 + .../agl_estimator.cpp.gcov.overview.html | 45 + .../estimators/agl_estimator.cpp.gcov.png | Bin 0 -> 454 bytes .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimation_manager/estimators/index.html | 112 + .../state_estimator.cpp.func-sort-c.html | 120 + .../estimators/state_estimator.cpp.func.html | 120 + .../state_estimator.cpp.gcov.frameset.html | 19 + .../estimators/state_estimator.cpp.gcov.html | 263 + .../state_estimator.cpp.gcov.overview.html | 65 + .../estimators/state_estimator.cpp.gcov.png | Bin 0 -> 753 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../src/estimation_manager/index-detail.html | 128 + .../src/estimation_manager/index-sort-f.html | 112 + .../src/estimation_manager/index-sort-l.html | 112 + .../src/estimation_manager/index.html | 112 + .../src/gain_manager.cpp.func-sort-c.html | 108 + .../src/gain_manager.cpp.func.html | 108 + .../src/gain_manager.cpp.gcov.frameset.html | 19 + .../src/gain_manager.cpp.gcov.html | 720 ++ .../src/gain_manager.cpp.gcov.overview.html | 179 + .../src/gain_manager.cpp.gcov.png | Bin 0 -> 2099 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 | 132 + .../transform_manager.cpp.func.html | 132 + .../transform_manager.cpp.gcov.frameset.html | 19 + .../transform_manager.cpp.gcov.html | 1030 ++ .../transform_manager.cpp.gcov.overview.html | 257 + .../transform_manager.cpp.gcov.png | Bin 0 -> 3258 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 | 2733 +++++ .../src/uav_manager.cpp.gcov.overview.html | 683 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7236 bytes .../agl/garmin_agl.h.func-sort-c.html | 92 + .../estimators/agl/garmin_agl.h.func.html | 92 + .../agl/garmin_agl.h.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.h.gcov.html | 159 + .../agl/garmin_agl.h.gcov.overview.html | 39 + .../estimators/agl/garmin_agl.h.gcov.png | Bin 0 -> 380 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../estimators/agl/index-detail.html | 110 + .../estimators/agl/index-sort-f.html | 102 + .../estimators/agl/index-sort-l.html | 102 + .../estimators/agl/index.html | 102 + .../altitude/alt_generic.h.func-sort-c.html | 92 + .../altitude/alt_generic.h.func.html | 92 + .../altitude/alt_generic.h.gcov.frameset.html | 19 + .../altitude/alt_generic.h.gcov.html | 242 + .../altitude/alt_generic.h.gcov.overview.html | 60 + .../altitude/alt_generic.h.gcov.png | Bin 0 -> 682 bytes .../altitude_estimator.h.func-sort-c.html | 92 + .../altitude/altitude_estimator.h.func.html | 92 + .../altitude_estimator.h.gcov.frameset.html | 19 + .../altitude/altitude_estimator.h.gcov.html | 130 + .../altitude_estimator.h.gcov.overview.html | 32 + .../altitude/altitude_estimator.h.gcov.png | Bin 0 -> 295 bytes .../altitude/index-detail-sort-f.html | 128 + .../altitude/index-detail-sort-l.html | 128 + .../estimators/altitude/index-detail.html | 128 + .../estimators/altitude/index-sort-f.html | 112 + .../estimators/altitude/index-sort-l.html | 112 + .../estimators/altitude/index.html | 112 + .../estimators/correction.h.func-sort-c.html | 348 + .../estimators/correction.h.func.html | 348 + .../correction.h.gcov.frameset.html | 19 + .../estimators/correction.h.gcov.html | 1795 ++++ .../correction.h.gcov.overview.html | 448 + .../estimators/correction.h.gcov.png | Bin 0 -> 5131 bytes .../heading/hdg_generic.h.func-sort-c.html | 92 + .../heading/hdg_generic.h.func.html | 92 + .../heading/hdg_generic.h.gcov.frameset.html | 19 + .../heading/hdg_generic.h.gcov.html | 240 + .../heading/hdg_generic.h.gcov.overview.html | 59 + .../estimators/heading/hdg_generic.h.gcov.png | Bin 0 -> 655 bytes .../hdg_passthrough.h.func-sort-c.html | 92 + .../heading/hdg_passthrough.h.func.html | 92 + .../hdg_passthrough.h.gcov.frameset.html | 19 + .../heading/hdg_passthrough.h.gcov.html | 191 + .../hdg_passthrough.h.gcov.overview.html | 47 + .../heading/hdg_passthrough.h.gcov.png | Bin 0 -> 506 bytes .../heading_estimator.h.func-sort-c.html | 84 + .../heading/heading_estimator.h.func.html | 84 + .../heading_estimator.h.gcov.frameset.html | 19 + .../heading/heading_estimator.h.gcov.html | 124 + .../heading_estimator.h.gcov.overview.html | 30 + .../heading/heading_estimator.h.gcov.png | Bin 0 -> 293 bytes .../heading/index-detail-sort-f.html | 138 + .../heading/index-detail-sort-l.html | 138 + .../estimators/heading/index-detail.html | 138 + .../estimators/heading/index-sort-f.html | 122 + .../estimators/heading/index-sort-l.html | 122 + .../estimators/heading/index.html | 122 + .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimators/index.html | 112 + .../lateral/index-detail-sort-f.html | 128 + .../lateral/index-detail-sort-l.html | 128 + .../estimators/lateral/index-detail.html | 128 + .../estimators/lateral/index-sort-f.html | 112 + .../estimators/lateral/index-sort-l.html | 112 + .../estimators/lateral/index.html | 112 + .../lateral/lat_generic.h.func-sort-c.html | 92 + .../lateral/lat_generic.h.func.html | 92 + .../lateral/lat_generic.h.gcov.frameset.html | 19 + .../lateral/lat_generic.h.gcov.html | 244 + .../lateral/lat_generic.h.gcov.overview.html | 60 + .../estimators/lateral/lat_generic.h.gcov.png | Bin 0 -> 682 bytes .../lateral_estimator.h.func-sort-c.html | 92 + .../lateral/lateral_estimator.h.func.html | 92 + .../lateral_estimator.h.gcov.frameset.html | 19 + .../lateral/lateral_estimator.h.gcov.html | 122 + .../lateral_estimator.h.gcov.overview.html | 30 + .../lateral/lateral_estimator.h.gcov.png | Bin 0 -> 284 bytes .../partial_estimator.h.func-sort-c.html | 176 + .../estimators/partial_estimator.h.func.html | 176 + .../partial_estimator.h.gcov.frameset.html | 19 + .../estimators/partial_estimator.h.gcov.html | 264 + .../partial_estimator.h.gcov.overview.html | 65 + .../estimators/partial_estimator.h.gcov.png | Bin 0 -> 820 bytes .../estimators/state/index-detail-sort-f.html | 128 + .../estimators/state/index-detail-sort-l.html | 128 + .../estimators/state/index-detail.html | 128 + .../estimators/state/index-sort-f.html | 112 + .../estimators/state/index-sort-l.html | 112 + .../estimators/state/index.html | 112 + .../state/passthrough.h.func-sort-c.html | 92 + .../estimators/state/passthrough.h.func.html | 92 + .../state/passthrough.h.gcov.frameset.html | 19 + .../estimators/state/passthrough.h.gcov.html | 173 + .../state/passthrough.h.gcov.overview.html | 43 + .../estimators/state/passthrough.h.gcov.png | Bin 0 -> 431 bytes .../state/state_generic.h.func-sort-c.html | 92 + .../state/state_generic.h.func.html | 92 + .../state/state_generic.h.gcov.frameset.html | 19 + .../state/state_generic.h.gcov.html | 183 + .../state/state_generic.h.gcov.overview.html | 45 + .../estimators/state/state_generic.h.gcov.png | Bin 0 -> 438 bytes .../processors/index-detail-sort-f.html | 182 + .../processors/index-detail-sort-l.html | 182 + .../processors/index-detail.html | 182 + .../processors/index-sort-f.html | 142 + .../processors/index-sort-l.html | 142 + .../processors/index.html | 142 + .../proc_excessive_tilt.h.func-sort-c.html | 104 + .../proc_excessive_tilt.h.func.html | 104 + .../proc_excessive_tilt.h.gcov.frameset.html | 19 + .../proc_excessive_tilt.h.gcov.html | 206 + .../proc_excessive_tilt.h.gcov.overview.html | 51 + .../processors/proc_excessive_tilt.h.gcov.png | Bin 0 -> 661 bytes .../proc_median_filter.h.func-sort-c.html | 104 + .../processors/proc_median_filter.h.func.html | 104 + .../proc_median_filter.h.gcov.frameset.html | 19 + .../processors/proc_median_filter.h.gcov.html | 192 + .../proc_median_filter.h.gcov.overview.html | 47 + .../processors/proc_median_filter.h.gcov.png | Bin 0 -> 627 bytes .../proc_saturate.h.func-sort-c.html | 104 + .../processors/proc_saturate.h.func.html | 104 + .../proc_saturate.h.gcov.frameset.html | 19 + .../processors/proc_saturate.h.gcov.html | 202 + .../proc_saturate.h.gcov.overview.html | 50 + .../processors/proc_saturate.h.gcov.png | Bin 0 -> 711 bytes .../proc_tf_to_world.h.func-sort-c.html | 112 + .../processors/proc_tf_to_world.h.func.html | 112 + .../proc_tf_to_world.h.gcov.frameset.html | 19 + .../processors/proc_tf_to_world.h.gcov.html | 222 + .../proc_tf_to_world.h.gcov.overview.html | 55 + .../processors/proc_tf_to_world.h.gcov.png | Bin 0 -> 689 bytes .../processors/processor.h.func-sort-c.html | 112 + .../processors/processor.h.func.html | 112 + .../processors/processor.h.gcov.frameset.html | 19 + .../processors/processor.h.gcov.html | 156 + .../processors/processor.h.gcov.overview.html | 38 + .../processors/processor.h.gcov.png | Bin 0 -> 436 bytes .../agl/garmin_agl.cpp.func-sort-c.html | 120 + .../estimators/agl/garmin_agl.cpp.func.html | 120 + .../agl/garmin_agl.cpp.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.cpp.gcov.html | 318 + .../agl/garmin_agl.cpp.gcov.overview.html | 79 + .../estimators/agl/garmin_agl.cpp.gcov.png | Bin 0 -> 1050 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../src/estimators/agl/index-detail.html | 110 + .../src/estimators/agl/index-sort-f.html | 102 + .../src/estimators/agl/index-sort-l.html | 102 + .../src/estimators/agl/index.html | 102 + .../altitude/alt_generic.cpp.func-sort-c.html | 200 + .../altitude/alt_generic.cpp.func.html | 200 + .../alt_generic.cpp.gcov.frameset.html | 19 + .../altitude/alt_generic.cpp.gcov.html | 797 ++ .../alt_generic.cpp.gcov.overview.html | 199 + .../altitude/alt_generic.cpp.gcov.png | Bin 0 -> 2797 bytes .../altitude/index-detail-sort-f.html | 110 + .../altitude/index-detail-sort-l.html | 110 + .../src/estimators/altitude/index-detail.html | 110 + .../src/estimators/altitude/index-sort-f.html | 102 + .../src/estimators/altitude/index-sort-l.html | 102 + .../src/estimators/altitude/index.html | 102 + .../heading/hdg_generic.cpp.func-sort-c.html | 204 + .../heading/hdg_generic.cpp.func.html | 204 + .../hdg_generic.cpp.gcov.frameset.html | 19 + .../heading/hdg_generic.cpp.gcov.html | 754 ++ .../hdg_generic.cpp.gcov.overview.html | 188 + .../heading/hdg_generic.cpp.gcov.png | Bin 0 -> 2195 bytes .../hdg_passthrough.cpp.func-sort-c.html | 172 + .../heading/hdg_passthrough.cpp.func.html | 172 + .../hdg_passthrough.cpp.gcov.frameset.html | 19 + .../heading/hdg_passthrough.cpp.gcov.html | 387 + .../hdg_passthrough.cpp.gcov.overview.html | 96 + .../heading/hdg_passthrough.cpp.gcov.png | Bin 0 -> 1189 bytes .../heading/index-detail-sort-f.html | 120 + .../heading/index-detail-sort-l.html | 120 + .../src/estimators/heading/index-detail.html | 120 + .../src/estimators/heading/index-sort-f.html | 112 + .../src/estimators/heading/index-sort-l.html | 112 + .../src/estimators/heading/index.html | 112 + .../lateral/index-detail-sort-f.html | 110 + .../lateral/index-detail-sort-l.html | 110 + .../src/estimators/lateral/index-detail.html | 110 + .../src/estimators/lateral/index-sort-f.html | 102 + .../src/estimators/lateral/index-sort-l.html | 102 + .../src/estimators/lateral/index.html | 102 + .../lateral/lat_generic.cpp.func-sort-c.html | 200 + .../lateral/lat_generic.cpp.func.html | 200 + .../lat_generic.cpp.gcov.frameset.html | 19 + .../lateral/lat_generic.cpp.gcov.html | 838 ++ .../lat_generic.cpp.gcov.overview.html | 209 + .../lateral/lat_generic.cpp.gcov.png | Bin 0 -> 2974 bytes .../state/gps_baro.cpp.func-sort-c.html | 96 + .../estimators/state/gps_baro.cpp.func.html | 96 + .../state/gps_baro.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_baro.cpp.gcov.html | 109 + .../state/gps_baro.cpp.gcov.overview.html | 27 + .../estimators/state/gps_baro.cpp.gcov.png | Bin 0 -> 231 bytes .../state/gps_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/gps_garmin.cpp.func.html | 96 + .../state/gps_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_garmin.cpp.gcov.html | 109 + .../state/gps_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/gps_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../estimators/state/index-detail-sort-f.html | 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 -> 1098 bytes .../estimators/state/rtk.cpp.func-sort-c.html | 96 + .../src/estimators/state/rtk.cpp.func.html | 96 + .../state/rtk.cpp.gcov.frameset.html | 19 + .../src/estimators/state/rtk.cpp.gcov.html | 109 + .../state/rtk.cpp.gcov.overview.html | 27 + .../src/estimators/state/rtk.cpp.gcov.png | Bin 0 -> 229 bytes .../state/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 -> 1971 bytes .../src/joy_tracker/index-detail-sort-f.html | 110 + .../src/joy_tracker/index-detail-sort-l.html | 110 + .../src/joy_tracker/index-detail.html | 110 + .../src/joy_tracker/index-sort-f.html | 102 + .../src/joy_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/joy_tracker/index.html | 102 + .../joy_tracker.cpp.func-sort-c.html | 152 + .../src/joy_tracker/joy_tracker.cpp.func.html | 152 + .../joy_tracker.cpp.gcov.frameset.html | 19 + .../src/joy_tracker/joy_tracker.cpp.gcov.html | 588 ++ .../joy_tracker.cpp.gcov.overview.html | 146 + .../src/joy_tracker/joy_tracker.cpp.gcov.png | Bin 0 -> 1740 bytes .../landoff_tracker/index-detail-sort-f.html | 110 + .../landoff_tracker/index-detail-sort-l.html | 110 + .../src/landoff_tracker/index-detail.html | 110 + .../src/landoff_tracker/index-sort-f.html | 102 + .../src/landoff_tracker/index-sort-l.html | 102 + .../src/landoff_tracker/index.html | 102 + .../landoff_tracker.cpp.func-sort-c.html | 204 + .../landoff_tracker.cpp.func.html | 204 + .../landoff_tracker.cpp.gcov.frameset.html | 19 + .../landoff_tracker.cpp.gcov.html | 1636 +++ .../landoff_tracker.cpp.gcov.overview.html | 408 + .../landoff_tracker.cpp.gcov.png | Bin 0 -> 4772 bytes .../src/line_tracker/index-detail-sort-f.html | 110 + .../src/line_tracker/index-detail-sort-l.html | 110 + .../src/line_tracker/index-detail.html | 110 + .../src/line_tracker/index-sort-f.html | 102 + .../src/line_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/line_tracker/index.html | 102 + .../line_tracker.cpp.func-sort-c.html | 200 + .../line_tracker/line_tracker.cpp.func.html | 200 + .../line_tracker.cpp.gcov.frameset.html | 19 + .../line_tracker/line_tracker.cpp.gcov.html | 1359 +++ .../line_tracker.cpp.gcov.overview.html | 339 + .../line_tracker/line_tracker.cpp.gcov.png | Bin 0 -> 3780 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../index-detail.html | 110 + .../index-sort-f.html | 102 + .../index-sort-l.html | 102 + .../src/midair_activation_tracker/index.html | 102 + ...ir_activation_tracker.cpp.func-sort-c.html | 152 + .../midair_activation_tracker.cpp.func.html | 152 + ..._activation_tracker.cpp.gcov.frameset.html | 19 + .../midair_activation_tracker.cpp.gcov.html | 426 + ..._activation_tracker.cpp.gcov.overview.html | 106 + .../midair_activation_tracker.cpp.gcov.png | Bin 0 -> 1153 bytes .../src/mpc_tracker/index-detail-sort-f.html | 110 + .../src/mpc_tracker/index-detail-sort-l.html | 110 + .../src/mpc_tracker/index-detail.html | 110 + .../src/mpc_tracker/index-sort-f.html | 102 + .../src/mpc_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/mpc_tracker/index.html | 102 + .../mpc_tracker.cpp.func-sort-c.html | 276 + .../src/mpc_tracker/mpc_tracker.cpp.func.html | 276 + .../mpc_tracker.cpp.gcov.frameset.html | 19 + .../src/mpc_tracker/mpc_tracker.cpp.gcov.html | 3966 ++++++++ .../mpc_tracker.cpp.gcov.overview.html | 991 ++ .../src/mpc_tracker/mpc_tracker.cpp.gcov.png | Bin 0 -> 12605 bytes .../speed_tracker/index-detail-sort-f.html | 110 + .../speed_tracker/index-detail-sort-l.html | 110 + .../src/speed_tracker/index-detail.html | 110 + .../src/speed_tracker/index-sort-f.html | 102 + .../src/speed_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/speed_tracker/index.html | 102 + .../speed_tracker.cpp.func-sort-c.html | 156 + .../speed_tracker/speed_tracker.cpp.func.html | 156 + .../speed_tracker.cpp.gcov.frameset.html | 19 + .../speed_tracker/speed_tracker.cpp.gcov.html | 1178 +++ .../speed_tracker.cpp.gcov.overview.html | 294 + .../speed_tracker/speed_tracker.cpp.gcov.png | Bin 0 -> 3011 bytes .../eth_mav_msgs/common.h.func-sort-c.html | 88 + .../include/eth_mav_msgs/common.h.func.html | 88 + .../eth_mav_msgs/common.h.gcov.frameset.html | 19 + .../include/eth_mav_msgs/common.h.gcov.html | 496 + .../eth_mav_msgs/common.h.gcov.overview.html | 123 + .../include/eth_mav_msgs/common.h.gcov.png | Bin 0 -> 1579 bytes .../eigen_mav_msgs.h.func-sort-c.html | 84 + .../eth_mav_msgs/eigen_mav_msgs.h.func.html | 84 + .../eigen_mav_msgs.h.gcov.frameset.html | 19 + .../eth_mav_msgs/eigen_mav_msgs.h.gcov.html | 463 + .../eigen_mav_msgs.h.gcov.overview.html | 115 + .../eth_mav_msgs/eigen_mav_msgs.h.gcov.png | Bin 0 -> 1483 bytes .../eth_mav_msgs/index-detail-sort-f.html | 128 + .../eth_mav_msgs/index-detail-sort-l.html | 128 + .../include/eth_mav_msgs/index-detail.html | 128 + .../include/eth_mav_msgs/index-sort-f.html | 112 + .../include/eth_mav_msgs/index-sort-l.html | 112 + .../include/eth_mav_msgs/index.html | 112 + .../extremum.h.func-sort-c.html | 80 + .../extremum.h.func.html | 80 + .../extremum.h.gcov.frameset.html | 19 + .../extremum.h.gcov.html | 143 + .../extremum.h.gcov.overview.html | 35 + .../extremum.h.gcov.png | Bin 0 -> 437 bytes .../impl/index-detail-sort-f.html | 128 + .../impl/index-detail-sort-l.html | 128 + .../impl/index-detail.html | 128 + .../impl/index-sort-f.html | 112 + .../impl/index-sort-l.html | 112 + .../eth_trajectory_generation/impl/index.html | 112 + ...ptimization_linear_impl.h.func-sort-c.html | 136 + ...omial_optimization_linear_impl.h.func.html | 136 + ...imization_linear_impl.h.gcov.frameset.html | 19 + ...omial_optimization_linear_impl.h.gcov.html | 708 ++ ...imization_linear_impl.h.gcov.overview.html | 176 + ...nomial_optimization_linear_impl.h.gcov.png | Bin 0 -> 2760 bytes ...mization_nonlinear_impl.h.func-sort-c.html | 152 + ...al_optimization_nonlinear_impl.h.func.html | 152 + ...zation_nonlinear_impl.h.gcov.frameset.html | 19 + ...al_optimization_nonlinear_impl.h.gcov.html | 933 ++ ...zation_nonlinear_impl.h.gcov.overview.html | 233 + ...ial_optimization_nonlinear_impl.h.gcov.png | Bin 0 -> 3527 bytes .../index-detail-sort-f.html | 228 + .../index-detail-sort-l.html | 228 + .../index-detail.html | 228 + .../index-sort-f.html | 172 + .../index-sort-l.html | 172 + .../eth_trajectory_generation/index.html | 172 + .../polynomial.h.func-sort-c.html | 104 + .../polynomial.h.func.html | 104 + .../polynomial.h.gcov.frameset.html | 19 + .../polynomial.h.gcov.html | 353 + .../polynomial.h.gcov.overview.html | 88 + .../polynomial.h.gcov.png | Bin 0 -> 1546 bytes ...ial_optimization_linear.h.func-sort-c.html | 84 + ...polynomial_optimization_linear.h.func.html | 84 + ...l_optimization_linear.h.gcov.frameset.html | 19 + ...polynomial_optimization_linear.h.gcov.html | 401 + ...l_optimization_linear.h.gcov.overview.html | 100 + .../polynomial_optimization_linear.h.gcov.png | Bin 0 -> 1756 bytes ..._optimization_nonlinear.h.func-sort-c.html | 80 + ...ynomial_optimization_nonlinear.h.func.html | 80 + ...ptimization_nonlinear.h.gcov.frameset.html | 19 + ...ynomial_optimization_nonlinear.h.gcov.html | 399 + ...ptimization_nonlinear.h.gcov.overview.html | 99 + ...lynomial_optimization_nonlinear.h.gcov.png | Bin 0 -> 1665 bytes .../segment.h.func-sort-c.html | 84 + .../segment.h.func.html | 84 + .../segment.h.gcov.frameset.html | 19 + .../segment.h.gcov.html | 231 + .../segment.h.gcov.overview.html | 57 + .../segment.h.gcov.png | Bin 0 -> 899 bytes .../timing.h.func-sort-c.html | 88 + .../timing.h.func.html | 88 + .../timing.h.gcov.frameset.html | 19 + .../timing.h.gcov.html | 322 + .../timing.h.gcov.overview.html | 80 + .../timing.h.gcov.png | Bin 0 -> 1036 bytes .../trajectory.h.func-sort-c.html | 92 + .../trajectory.h.func.html | 92 + .../trajectory.h.gcov.frameset.html | 19 + .../trajectory.h.gcov.html | 261 + .../trajectory.h.gcov.overview.html | 65 + .../trajectory.h.gcov.png | Bin 0 -> 979 bytes .../vertex.h.func-sort-c.html | 80 + .../vertex.h.func.html | 80 + .../vertex.h.gcov.frameset.html | 19 + .../vertex.h.gcov.html | 256 + .../vertex.h.gcov.overview.html | 63 + .../vertex.h.gcov.png | Bin 0 -> 1058 bytes .../index-detail-sort-f.html | 202 + .../index-detail-sort-l.html | 202 + .../index-detail.html | 202 + .../index-sort-f.html | 162 + .../index-sort-l.html | 162 + .../src/eth_trajectory_generation/index.html | 162 + .../motion_defines.cpp.func-sort-c.html | 96 + .../motion_defines.cpp.func.html | 96 + .../motion_defines.cpp.gcov.frameset.html | 19 + .../motion_defines.cpp.gcov.html | 174 + .../motion_defines.cpp.gcov.overview.html | 43 + .../motion_defines.cpp.gcov.png | Bin 0 -> 468 bytes .../polynomial.cpp.func-sort-c.html | 124 + .../polynomial.cpp.func.html | 124 + .../polynomial.cpp.gcov.frameset.html | 19 + .../polynomial.cpp.gcov.html | 325 + .../polynomial.cpp.gcov.overview.html | 81 + .../polynomial.cpp.gcov.png | Bin 0 -> 1168 bytes .../rpoly/index-detail-sort-f.html | 110 + .../rpoly/index-detail-sort-l.html | 110 + .../rpoly/index-detail.html | 110 + .../rpoly/index-sort-f.html | 102 + .../rpoly/index-sort-l.html | 102 + .../rpoly/index.html | 102 + .../rpoly/rpoly_ak1.cpp.func-sort-c.html | 128 + .../rpoly/rpoly_ak1.cpp.func.html | 128 + .../rpoly/rpoly_ak1.cpp.gcov.frameset.html | 19 + .../rpoly/rpoly_ak1.cpp.gcov.html | 1027 ++ .../rpoly/rpoly_ak1.cpp.gcov.overview.html | 256 + .../rpoly/rpoly_ak1.cpp.gcov.png | Bin 0 -> 4794 bytes .../segment.cpp.func-sort-c.html | 132 + .../segment.cpp.func.html | 132 + .../segment.cpp.gcov.frameset.html | 19 + .../segment.cpp.gcov.html | 399 + .../segment.cpp.gcov.overview.html | 99 + .../segment.cpp.gcov.png | Bin 0 -> 1506 bytes .../timing.cpp.func-sort-c.html | 200 + .../timing.cpp.func.html | 200 + .../timing.cpp.gcov.frameset.html | 19 + .../timing.cpp.gcov.html | 415 + .../timing.cpp.gcov.overview.html | 103 + .../timing.cpp.gcov.png | Bin 0 -> 1087 bytes .../trajectory.cpp.func-sort-c.html | 172 + .../trajectory.cpp.func.html | 172 + .../trajectory.cpp.gcov.frameset.html | 19 + .../trajectory.cpp.gcov.html | 780 ++ .../trajectory.cpp.gcov.overview.html | 194 + .../trajectory.cpp.gcov.png | Bin 0 -> 2540 bytes .../trajectory_sampling.cpp.func-sort-c.html | 108 + .../trajectory_sampling.cpp.func.html | 108 + ...trajectory_sampling.cpp.gcov.frameset.html | 19 + .../trajectory_sampling.cpp.gcov.html | 271 + ...trajectory_sampling.cpp.gcov.overview.html | 67 + .../trajectory_sampling.cpp.gcov.png | Bin 0 -> 941 bytes .../vertex.cpp.func-sort-c.html | 148 + .../vertex.cpp.func.html | 148 + .../vertex.cpp.gcov.frameset.html | 19 + .../vertex.cpp.gcov.html | 670 ++ .../vertex.cpp.gcov.overview.html | 167 + .../vertex.cpp.gcov.png | Bin 0 -> 2242 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + .../src/index-detail.html | 110 + .../src/index-sort-f.html | 102 + .../src/index-sort-l.html | 102 + mrs_uav_trajectory_generation/src/index.html | 102 + ...trajectory_generation.cpp.func-sort-c.html | 172 + .../mrs_trajectory_generation.cpp.func.html | 172 + ...ajectory_generation.cpp.gcov.frameset.html | 19 + .../mrs_trajectory_generation.cpp.gcov.html | 2408 +++++ ...ajectory_generation.cpp.gcov.overview.html | 601 ++ .../mrs_trajectory_generation.cpp.gcov.png | Bin 0 -> 7472 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 1158 files changed, 196605 insertions(+) create mode 100644 .nojekyll create mode 100644 amber.png create mode 100644 badge.svg create mode 100644 emerald.png create mode 100644 gcov.css create mode 100644 glass.png create mode 100644 index-sort-f.html create mode 100644 index-sort-l.html create mode 100644 index.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/utils.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.func.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.png create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.png create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index-detail.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/conversions.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.func.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/src/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/src/geometry/index-detail.html create mode 100644 mrs_lib/src/geometry/index-sort-f.html create mode 100644 mrs_lib/src/geometry/index-sort-l.html create mode 100644 mrs_lib/src/geometry/index.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/shapes.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.func.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.png create mode 100644 mrs_lib/src/math/index-detail-sort-f.html create mode 100644 mrs_lib/src/math/index-detail-sort-l.html create mode 100644 mrs_lib/src/math/index-detail.html create mode 100644 mrs_lib/src/math/index-sort-f.html create mode 100644 mrs_lib/src/math/index-sort-l.html create mode 100644 mrs_lib/src/math/index.html create mode 100644 mrs_lib/src/math/math.cpp.func-sort-c.html create mode 100644 mrs_lib/src/math/math.cpp.func.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.overview.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.png create mode 100644 mrs_lib/src/median_filter/index-detail-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-detail-sort-l.html create mode 100644 mrs_lib/src/median_filter/index-detail.html create mode 100644 mrs_lib/src/median_filter/index-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-sort-l.html create mode 100644 mrs_lib/src/median_filter/index.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.png create mode 100644 mrs_lib/src/param_loader/index-detail-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-detail-sort-l.html create mode 100644 mrs_lib/src/param_loader/index-detail.html create mode 100644 mrs_lib/src/param_loader/index-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-sort-l.html create mode 100644 mrs_lib/src/param_loader/index.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.png create mode 100644 mrs_lib/src/profiler/index-detail-sort-f.html create mode 100644 mrs_lib/src/profiler/index-detail-sort-l.html create mode 100644 mrs_lib/src/profiler/index-detail.html create mode 100644 mrs_lib/src/profiler/index-sort-f.html create mode 100644 mrs_lib/src/profiler/index-sort-l.html create mode 100644 mrs_lib/src/profiler/index.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func-sort-c.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.overview.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index-detail.html create mode 100644 mrs_lib/src/safety_zone/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index-detail.html create mode 100644 mrs_lib/src/scope_timer/index-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index-detail.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png create mode 100644 mrs_lib/src/timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/timer/index-detail.html create mode 100644 mrs_lib/src/timer/index-sort-f.html create mode 100644 mrs_lib/src/timer/index-sort-l.html create mode 100644 mrs_lib/src/timer/index.html create mode 100644 mrs_lib/src/timer/timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timer/timer.cpp.func.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.png create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png create mode 100644 mrs_lib/src/transformer/index-detail-sort-f.html create mode 100644 mrs_lib/src/transformer/index-detail-sort-l.html create mode 100644 mrs_lib/src/transformer/index-detail.html create mode 100644 mrs_lib/src/transformer/index-sort-f.html create mode 100644 mrs_lib/src/transformer/index-sort-l.html create mode 100644 mrs_lib/src/transformer/index.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.png create mode 100644 mrs_lib/src/utils/index-detail-sort-f.html create mode 100644 mrs_lib/src/utils/index-detail-sort-l.html create mode 100644 mrs_lib/src/utils/index-detail.html create mode 100644 mrs_lib/src/utils/index-sort-f.html create mode 100644 mrs_lib/src/utils/index-sort-l.html create mode 100644 mrs_lib/src/utils/index.html create mode 100644 mrs_lib/src/utils/utils.cpp.func-sort-c.html create mode 100644 mrs_lib/src/utils/utils.cpp.func.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.overview.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func-sort-c.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/index-detail-sort-f.html create mode 100644 mrs_uav_autostart/src/index-detail-sort-l.html create mode 100644 mrs_uav_autostart/src/index-detail.html create mode 100644 mrs_uav_autostart/src/index-sort-f.html create mode 100644 mrs_uav_autostart/src/index-sort-l.html create mode 100644 mrs_uav_autostart/src/index.html create mode 100644 mrs_uav_controllers/include/common.h.func-sort-c.html create mode 100644 mrs_uav_controllers/include/common.h.func.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.overview.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.png create mode 100644 mrs_uav_controllers/include/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/include/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/include/index-detail.html create mode 100644 mrs_uav_controllers/include/index-sort-f.html create mode 100644 mrs_uav_controllers/include/index-sort-l.html create mode 100644 mrs_uav_controllers/include/index.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func-sort-c.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.overview.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.png create mode 100644 mrs_uav_controllers/src/common.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/common.cpp.func.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/src/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/src/index-detail.html create mode 100644 mrs_uav_controllers/src/index-sort-f.html create mode 100644 mrs_uav_controllers/src/index-sort-l.html create mode 100644 mrs_uav_controllers/src/index.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.png create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/index-detail.html create mode 100644 mrs_uav_managers/src/index-sort-f.html create mode 100644 mrs_uav_managers/src/index-sort-l.html create mode 100644 mrs_uav_managers/src/index.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.png create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/index.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png create mode 100644 ruby.png create mode 100644 snow.png create mode 100644 updown.png diff --git a/.nojekyll b/.nojekyll new file mode 100644 index 0000000000..e69de29bb2 diff --git a/amber.png b/amber.png new file mode 100644 index 0000000000000000000000000000000000000000..2cab170d8359081983a4e343848dfe06bc490f12 GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^j3CU&3?x-=hn)ga>?NMQuI!iC1^G2tW}LqE04T&+ z;1OBOz`!j8!i<;h*8KqrvZOouIx;Y9?C1WI$O`1M1^9%x{(levWGtest coveragetest coverage59.5%59.5% \ 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..f86ff66a09 --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,662 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:145822320262.8 %
Date:2024-01-07 21:47:14Functions:2476416159.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/geometry +
14.6%14.6%
+ 		14.6 %65 / 44514.9 %14 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.9%18.9%
+ 		18.9 %88 / 46622.2 %12 / 54
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
45.1%45.1%
+ 		45.1 %448 / 99429.5 %31 / 105
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9340.8 %49 / 120
mrs_lib/src/batch_visualizer +
47.1%47.1%
+ 		47.1 %192 / 40846.5 %20 / 43
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
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 +
75.3%75.3%
+ 		75.3 %131 / 17450.0 %17 / 34
mrs_lib/include/mrs_lib/impl +
80.3%80.3%
+ 		80.3 %342 / 42655.6 %731 / 1315
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
50.2%50.2%
+ 		50.2 %314 / 62559.4 %19 / 32
mrs_lib/include/mrs_lib +
77.7%77.7%
+ 		77.7 %748 / 96359.3 %759 / 1279
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_state_estimators/src/estimators/agl +
70.2%70.2%
+ 		70.2 %73 / 10460.0 %6 / 10
mrs_uav_trackers/src/mpc_tracker +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_managers/src/control_manager +
59.1%59.1%
+ 		59.1 %2185 / 369966.4 %81 / 122
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_managers/src/control_manager/common +
40.7%40.7%
+ 		40.7 %229 / 56367.7 %21 / 31
mrs_uav_trackers/src/landoff_tracker +
72.1%72.1%
+ 		72.1 %427 / 59267.7 %21 / 31
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 +
59.6%59.6%
+ 		59.6 %412 / 69172.5 %66 / 91
mrs_uav_state_estimators/src/estimators/altitude +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
mrs_uav_managers/include/transform_manager +
52.0%52.0%
+ 		52.0 %207 / 39873.7 %14 / 19
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
64.9%64.9%
+ 		64.9 %120 / 18576.9 %10 / 13
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_managers/src +
62.9%62.9%
+ 		62.9 %1063 / 169078.1 %57 / 73
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/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/src/estimators/lateral +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
mrs_uav_managers/src/estimation_manager +
69.5%69.5%
+ 		69.5 %426 / 61385.1 %40 / 47
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1748 / 217287.9 %58 / 66
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/profiler +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
mrs_uav_trajectory_generation/src +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
mrs_uav_managers/src/transform_manager +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
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_uav_trajectory_generation/include/eth_mav_msgs +
100.0%
+ 		100.0 %28 / 28100.0 %3 / 3
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
mrs_uav_autostart/src +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index-sort-l.html b/index-sort-l.html new file mode 100644 index 0000000000..5b768e329b --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,662 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:145822320262.8 %
Date:2024-01-07 21:47:14Functions:2476416159.5 %
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_lib/src/geometry +
14.6%14.6%
+ 		14.6 %65 / 44514.9 %14 / 94
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_state_estimators/src/estimators/heading +
18.9%18.9%
+ 		18.9 %88 / 46622.2 %12 / 54
mrs_lib/src/profiler +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
mrs_uav_managers/src/control_manager/common +
40.7%40.7%
+ 		40.7 %229 / 56367.7 %21 / 31
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
45.1%45.1%
+ 		45.1 %448 / 99429.5 %31 / 105
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_lib/src/batch_visualizer +
47.1%47.1%
+ 		47.1 %192 / 40846.5 %20 / 43
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
50.2%50.2%
+ 		50.2 %314 / 62559.4 %19 / 32
mrs_uav_managers/include/transform_manager +
52.0%52.0%
+ 		52.0 %207 / 39873.7 %14 / 19
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
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/lateral +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_managers/src/control_manager +
59.1%59.1%
+ 		59.1 %2185 / 369966.4 %81 / 122
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
59.6%59.6%
+ 		59.6 %412 / 69172.5 %66 / 91
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_trajectory_generation/src +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
mrs_uav_state_estimators/src/estimators/altitude +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
mrs_uav_managers/src +
62.9%62.9%
+ 		62.9 %1063 / 169078.1 %57 / 73
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
64.9%64.9%
+ 		64.9 %120 / 18576.9 %10 / 13
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_managers/src/estimation_manager +
69.5%69.5%
+ 		69.5 %426 / 61385.1 %40 / 47
mrs_uav_state_estimators/src/estimators/agl +
70.2%70.2%
+ 		70.2 %73 / 10460.0 %6 / 10
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 / 9340.8 %49 / 120
mrs_uav_trackers/src/mpc_tracker +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
mrs_uav_trackers/src/landoff_tracker +
72.1%72.1%
+ 		72.1 %427 / 59267.7 %21 / 31
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 +
75.3%75.3%
+ 		75.3 %131 / 17450.0 %17 / 34
mrs_lib/include/mrs_lib +
77.7%77.7%
+ 		77.7 %748 / 96359.3 %759 / 1279
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_lib/include/mrs_lib/impl +
80.3%80.3%
+ 		80.3 %342 / 42655.6 %731 / 1315
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1748 / 217287.9 %58 / 66
mrs_uav_managers/src/transform_manager +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
mrs_uav_autostart/src +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
mrs_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_uav_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_uav_trajectory_generation/include/eth_mav_msgs +
100.0%
+ 		100.0 %28 / 28100.0 %3 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index.html b/index.html new file mode 100644 index 0000000000..144490698e --- /dev/null +++ b/index.html @@ -0,0 +1,662 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:145822320262.8 %
Date:2024-01-07 21:47:14Functions:2476416159.5 %
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 +
77.7%77.7%
+ 		77.7 %748 / 96359.3 %759 / 1279
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9340.8 %49 / 120
mrs_lib/include/mrs_lib/impl +
80.3%80.3%
+ 		80.3 %342 / 42655.6 %731 / 1315
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_lib/src/batch_visualizer +
47.1%47.1%
+ 		47.1 %192 / 40846.5 %20 / 43
mrs_lib/src/geometry +
14.6%14.6%
+ 		14.6 %65 / 44514.9 %14 / 94
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_lib/src/profiler +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
mrs_uav_autostart/src +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
mrs_uav_controllers/include +
94.8%94.8%
+ 		94.8 %55 / 58100.0 %14 / 14
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1748 / 217287.9 %58 / 66
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_managers/include/mrs_uav_managers/estimation_manager +
97.2%97.2%
+ 		97.2 %106 / 10994.4 %17 / 18
mrs_uav_managers/include/transform_manager +
52.0%52.0%
+ 		52.0 %207 / 39873.7 %14 / 19
mrs_uav_managers/src +
62.9%62.9%
+ 		62.9 %1063 / 169078.1 %57 / 73
mrs_uav_managers/src/control_manager +
59.1%59.1%
+ 		59.1 %2185 / 369966.4 %81 / 122
mrs_uav_managers/src/control_manager/common +
40.7%40.7%
+ 		40.7 %229 / 56367.7 %21 / 31
mrs_uav_managers/src/estimation_manager +
69.5%69.5%
+ 		69.5 %426 / 61385.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 +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
59.6%59.6%
+ 		59.6 %412 / 69172.5 %66 / 91
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
75.3%75.3%
+ 		75.3 %131 / 17450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
70.2%70.2%
+ 		70.2 %73 / 10460.0 %6 / 10
mrs_uav_state_estimators/src/estimators/altitude +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
mrs_uav_state_estimators/src/estimators/heading +
18.9%18.9%
+ 		18.9 %88 / 46622.2 %12 / 54
mrs_uav_state_estimators/src/estimators/lateral +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
mrs_uav_state_estimators/src/estimators/state +
70.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 +
72.1%72.1%
+ 		72.1 %427 / 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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trajectory_generation/include/eth_mav_msgs +
100.0%
+ 		100.0 %28 / 28100.0 %3 / 3
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
64.9%64.9%
+ 		64.9 %120 / 18576.9 %10 / 13
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
50.2%50.2%
+ 		50.2 %314 / 62559.4 %19 / 32
mrs_uav_trajectory_generation/src +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
45.1%45.1%
+ 		45.1 %448 / 99429.5 %31 / 105
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html new file mode 100644 index 0000000000..965f066e0d --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232979.3 %
Date:2024-01-07 21:47:14Functions:91275.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
mrs_lib::AttitudeConverter::EulerFormatException::what() const0
auto mrs_lib::AttitudeConverter::get<0ul>()1
auto mrs_lib::AttitudeConverter::get<1ul>()1
auto mrs_lib::AttitudeConverter::get<2ul>()1
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen9AngleAxisIT_EEIdEEv1
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv54456
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)180020
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)342836
+
+
+ + + +
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..a087107b9d --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232979.3 %
Date:2024-01-07 21:47:14Functions:91275.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)180020
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>)342836
mrs_lib::AttitudeConverter::MathErrorException::what() const0
mrs_lib::AttitudeConverter::GetHeadingException::what() const1
mrs_lib::AttitudeConverter::SetHeadingException::what() const0
mrs_lib::AttitudeConverter::EulerFormatException::what() const0
mrs_lib::AttitudeConverter::InvalidAttitudeException::what() const2
_ZNK7mrs_lib17AttitudeConvertercvN5Eigen10QuaternionIT_Li0EEEIdEEv54456
_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..515aba353b --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html @@ -0,0 +1,617 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232979.3 %
Date:2024-01-07 21:47:14Functions:91275.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..708a493ebf15f10db204e2e60e12329b8d395a7e GIT binary patch literal 1660 zcmV-?27~#DP)R0{{R3v^8aD0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpRn< zn<Azo}&sJLrpMfT}Pk+x>i}3hiljFHK z=1Nx63Ge-A7s_wY#}A;7K_$0eSJB5@;!Ad26IP|lfkJ+RKBhZ9E4{puJ|+}x(auDu z?H?sLe)+F(^ZMR@@_I9l>Kx;4hh8wGCYr@qx`f!S-&ue#U?kaB6m>R9jXC>0F*Gmv zD*IONA90PfVxLS6+4!nwu!Ld0RkLv=feXAR39S|zysf41%n*9@N_kZ z0nd1UO$<-I#!3b_^57YmmqdHo<|TtiWL_+IhUdx1@EACb*78CE4@_bhji)^$-%Q+{ zNuUg>$wxKo%?xoHXPSEKVf8W-QZE|vH62!(l%U}139qC)9-R^UkAz0S(hA;flFD%}&FKWlTp zTlI*9*sfdfv{^AS=rtKFNA}6bn?}T5Su@q&;*{M zu{0DpZou!0}f;Q=#?<=C6FFLhk`p zp7zw0`xGAoZtpz_n1d@;iU~*Zp}2HlDV=>YrNrQMfm7$Q@noICIr}(qQNA7_2XK(p zr&p|CkRwAMUGn)h;$IVDy9R(y2q_%%_UF*!vH;JsDTyakz->A5X~Nrm*Hn1ss|50X zxSkjIgJ;a~51t`MUT)^9{~=JF{CIEjQ9GTC6E~GrgvY=6U*W-XeD~t4BuNHfWNey2 zdV*x<05e?5#Q4cdQJvB-65qlE+}R!Gg$)_-sRY*nTnzUqR#+xs^4O4z%Ly$R@JKM9 z5Cbd110it=1;_6W9GNy_pd3|MfH0VnuXDxUaik6=21zg`TQ9p=gAv3v$WfwZ;V1lx z6GUE0#TDb|rj@G)ZMNg8f5IQ#&>kh0eqeD(T5M_+Sw$Rgc}Bxtlj(*NS9C!ONE5$u z@4k+7+!Kse)q8q9w?-|l1J5=s;VGx>wf&&fy<>{J{80000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-01-07 21:47:14Functions: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&)60
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)60
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)60
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&)60
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_defaults(mrs_uav_state_estimators::AltitudeEstimatorConfig&)107
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)107
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)107
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&)107
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)120
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&)180
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&)180
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<double>(boost::any&, double*&)180
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)180
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)214
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&)227
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&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_defaults(mrs_uav_state_estimators::CorrectionConfig&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)227
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<double>(boost::any&, double*&)227
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)227
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&)227
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&)321
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&)321
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<double>(boost::any&, double*&)321
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)321
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)454
+
+
+ + + +
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..914073fa77 --- /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-01-07 21:47:14Functions: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&)227
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&)227
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&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)454
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)227
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*&)227
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)227
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)227
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&)227
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&)180
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&)180
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&)60
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)120
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)60
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)60
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*&)180
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)180
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&)60
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&)321
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&)321
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&)107
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)214
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)107
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)107
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*&)321
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)321
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&)107
+
+
+ + + +
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..a203e1f102 --- /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-01-07 21:47:14Functions: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         394 :   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         394 :         m_server(m_server_mtx, nh),
+      54             :         m_usr_cbf(user_callback),
+      55         394 :         m_pl(nh, print_values, node_name)
+      56             :   {
+      57             :     // initialize the dynamic reconfigure callback
+      58         394 :     m_server.setCallback(boost::bind(&DynamicReconfigureMgr<ConfigType>::dynamic_reconfigure_callback, this, _1, _2));
+      59         394 :   };
+      60             : 
+      61             :   /* Constructor overloads //{ */
+      62             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(nh, node_name)
+      63         227 :   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         788 :   void update_config(const ConfigType& cfg)
+      72             :   {
+      73         788 :     m_server.updateConfig(cfg);
+      74         788 :   }
+      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         394 :   void dynamic_reconfigure_callback(ConfigType& new_config, uint32_t level)
+     107             :   {
+     108         394 :     if (m_print_values)
+     109             :     {
+     110         394 :       if (m_node_name.empty())
+     111           0 :         ROS_INFO("Dynamic reconfigure request received");
+     112             :       else
+     113         394 :         ROS_INFO("[%s]: Dynamic reconfigure request received", m_node_name.c_str());
+     114             :     }
+     115             : 
+     116         394 :     if (m_not_initialized)
+     117             :     {
+     118         394 :       load_defaults(new_config);
+     119         394 :       update_config(new_config);
+     120             :     }
+     121         394 :     if (m_print_values)
+     122             :     {
+     123         394 :       print_changed_params(new_config);
+     124             :     }
+     125         394 :     m_not_initialized = false;
+     126         394 :     config = new_config;
+     127         394 :     if (m_usr_cbf)
+     128         167 :       m_usr_cbf(new_config, level);
+     129         394 :   }
+     130             : 
+     131             :   template <typename T>
+     132         728 :   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        1456 :     boost::shared_ptr<const param_descr_t> cast_descr = boost::dynamic_pointer_cast<const param_descr_t>(descr);
+     136         728 :     m_pl.loadParam(name, config.*(cast_descr->field));
+     137         728 :   }
+     138             :   
+     139         394 :   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         788 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     143        1122 :     for (auto& descr : descrs)
+     144             :     {
+     145        1456 :       std::string name = descr->name;
+     146         728 :       size_t pos = name.find("__");
+     147         728 :       while (pos != name.npos)
+     148             :       {
+     149           0 :         name.replace(pos, 2, "/");
+     150           0 :         pos = name.find("__");
+     151             :       }
+     152             : 
+     153         728 :       if (descr->type == "bool")
+     154           0 :         load_param<bool>(name, descr, new_config);
+     155         728 :       else if (descr->type == "int")
+     156           0 :         load_param<int>(name, descr, new_config);
+     157         728 :       else if (descr->type == "double")
+     158         728 :         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         394 :   }
+     168             : 
+     169             :   // method for printing names and values of new received parameters (prints only the changed ones) //{
+     170         394 :   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         788 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     174        1122 :     for (auto& descr : descrs)
+     175             :     {
+     176           0 :       boost::any val, old_val;
+     177         728 :       descr->getValue(new_config, val);
+     178         728 :       descr->getValue(config, old_val);
+     179         728 :       std::string name = descr->name;
+     180         728 :       const size_t pos = name.find("__");
+     181         728 :       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         728 :       if (try_cast(val, intval))
+     199             :       {
+     200           0 :         if (m_not_initialized || !try_compare(old_val, intval))
+     201           0 :           print_value(name, *intval);
+     202         728 :       } else if (try_cast(val, doubleval))
+     203             :       {
+     204         728 :         if (m_not_initialized || !try_compare(old_val, doubleval))
+     205         728 :           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         394 :   }
+     220             :   //}
+     221             :   
+     222             :   // helper method for parameter printing
+     223             :   template <typename T>
+     224         728 :   inline void print_value(const std::string& name, const T& val)
+     225             :   {
+     226         728 :     if (m_node_name.empty())
+     227           0 :       std::cout << "\t" << name << ":\t" << val << std::endl;
+     228             :     else
+     229         728 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << val);
+     230         728 :   }
+     231             :   // helper methods for automatic parameter value parsing
+     232             :   template <typename T>
+     233        1456 :   inline bool try_cast(boost::any& val, T*& out)
+     234             :   {
+     235        1456 :     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..622b25a6dd --- /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-01-07 21:47:14Functions:4611739.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::sdegrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::sradians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::value() const0
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::convert<mrs_lib::geometry::radians>() const1
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::value() const4
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>&&)6
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)189
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::value() const10002
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10004
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10008
mrs_lib::geometry::operator-(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)10009
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10464
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>)224940
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)322315
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)393160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)393160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)393160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)429450
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)469566
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>)557271
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)806344
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1144324
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1167330
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)1920360
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)1932106
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>)2335026
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)4770049
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)5254466
+
+
+ + + +
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..66db9fcbb9 --- /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-01-07 21:47:14Functions:4611739.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)189
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>)557271
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)322315
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>)224940
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10464
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1167330
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)429450
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)469566
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1144324
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)393160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)393160
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>)2335026
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)1932106
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)10008
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)5254466
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)393160
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)1920360
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&)806344
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)4770049
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..46d9be9ad8 --- /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-01-07 21:47:14Functions:4611739.3 %
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     5956178 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67     1276563 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73      429450 :       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     6481801 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117     6481801 :         if (val >= minimum)
+     118             :         {
+     119     5981764 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120     5752923 :             return val;
+     121      226932 :           else if (val < supremum + range)
+     122       64525 :             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      500054 :           if (val >= minimum - range)
+     126      335761 :             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      326705 :         const flt rem = std::fmod(val - minimum, range);
+     131      326705 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326562 :         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     1950608 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159     1950608 :         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     2274421 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195     2274421 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198     2913197 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200     2913197 :         const flt d = minuend.val - subtrahend.val;
+     201     2913008 :         if (d < -half_range)
+     202       22921 :           return d + range;
+     203     2890276 :         if (d >= half_range)
+     204       18749 :           return d - range;
+     205     2870642 :         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       20464 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20464 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224      235412 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226      235412 :         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      393349 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244      393349 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247      393349 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249      393349 :         const flt dang = diff(to, from);
+     250      393349 :         const flt intp = from.val + coeff * dang;
+     251      393160 :         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      393349 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266      393349 :         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-01-07 21:47:14Functions:4912040.8 %
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 / 8739.3 %46 / 117
<unnamed>69.0 %60 / 8739.3 %46 / 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..68e19cf04e --- /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-01-07 21:47:14Functions:4912040.8 %
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 / 8739.3 %46 / 117
<unnamed>69.0 %60 / 8739.3 %46 / 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..ad7a38c612 --- /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-01-07 21:47:14Functions:4912040.8 %
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 / 8739.3 %46 / 117
<unnamed>69.0 %60 / 8739.3 %46 / 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..c48b7cabbc --- /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-01-07 21:47:14Functions:4912040.8 %
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 / 8739.3 %46 / 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..cad9962dc8 --- /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-01-07 21:47:14Functions:4912040.8 %
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 / 8739.3 %46 / 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..145bba1f0e --- /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-01-07 21:47:14Functions:4912040.8 %
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 / 8739.3 %46 / 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..e3220bf6f4 --- /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-01-07 21:47:14Functions: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&)63678
+
+
+ + + +
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..bfcb9b72e8 --- /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-01-07 21:47:14Functions: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&)63678
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..776752f1f2 --- /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-01-07 21:47:14Functions: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       63684 :     double headingFromRot(const T& rot)
+      58             :     {
+      59       63684 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60      127368 :       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-01-07 21:47:14Functions: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&)1025
mrs_lib::UTM(double, double, double*, double*)8773
mrs_lib::LLtoUTM(double, double, double&, double&, char*)108659
mrs_lib::UTMLetterDesignator(double)108663
+
+
+ + + +
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..357de440af --- /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-01-07 21:47:14Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)108663
mrs_lib::UTM(double, double, double*, double*)8773
mrs_lib::LLtoUTM(double, double, double&, double&, char*)108659
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)1025
+
+
+ + + +
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..ea81e1ec97 --- /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-01-07 21:47:14Functions: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        8773 :   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        8773 :     int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
+      63             : 
+      64             :     // convert degrees into radians
+      65        8773 :     double rlat  = lat * RADIANS_PER_DEGREE;
+      66        8773 :     double rlon  = lon * RADIANS_PER_DEGREE;
+      67        8773 :     double rlon0 = cm * RADIANS_PER_DEGREE;
+      68             : 
+      69             :     // compute trigonometric functions
+      70        8773 :     double slat = sin(rlat);
+      71        8773 :     double clat = cos(rlat);
+      72        8773 :     double tlat = tan(rlat);
+      73             : 
+      74             :     // decide the false northing at origin
+      75        8773 :     double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
+      76             : 
+      77        8773 :     double T = tlat * tlat;
+      78        8773 :     double C = UTM_EP2 * clat * clat;
+      79        8773 :     double A = (rlon - rlon0) * clat;
+      80        8773 :     double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
+      81        8773 :     double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
+      82             : 
+      83             :     // compute the easting-northing coordinates
+      84        8773 :     *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        8757 :     *y = fn +
+      86        8757 :          UTM_K0 *
+      87        8764 :              (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        8757 :     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      108663 :   static inline char UTMLetterDesignator(double Lat) {
+     102             :     char LetterDesignator;
+     103             : 
+     104      108663 :     if ((84 >= Lat) && (Lat >= 72))
+     105           0 :       LetterDesignator = 'X';
+     106      108663 :     else if ((72 > Lat) && (Lat >= 64))
+     107           0 :       LetterDesignator = 'W';
+     108      108663 :     else if ((64 > Lat) && (Lat >= 56))
+     109           0 :       LetterDesignator = 'V';
+     110      108663 :     else if ((56 > Lat) && (Lat >= 48))
+     111           0 :       LetterDesignator = 'U';
+     112      108663 :     else if ((48 > Lat) && (Lat >= 40))
+     113      108138 :       LetterDesignator = 'T';
+     114         525 :     else if ((40 > Lat) && (Lat >= 32))
+     115           0 :       LetterDesignator = 'S';
+     116         525 :     else if ((32 > Lat) && (Lat >= 24))
+     117           0 :       LetterDesignator = 'R';
+     118         525 :     else if ((24 > Lat) && (Lat >= 16))
+     119           0 :       LetterDesignator = 'Q';
+     120         525 :     else if ((16 > Lat) && (Lat >= 8))
+     121           0 :       LetterDesignator = 'P';
+     122         525 :     else if ((8 > Lat) && (Lat >= 0))
+     123           2 :       LetterDesignator = 'N';
+     124         523 :     else if ((0 > Lat) && (Lat >= -8))
+     125           0 :       LetterDesignator = 'M';
+     126         523 :     else if ((-8 > Lat) && (Lat >= -16))
+     127           0 :       LetterDesignator = 'L';
+     128         523 :     else if ((-16 > Lat) && (Lat >= -24))
+     129           0 :       LetterDesignator = 'K';
+     130         523 :     else if ((-24 > Lat) && (Lat >= -32))
+     131           0 :       LetterDesignator = 'J';
+     132         523 :     else if ((-32 > Lat) && (Lat >= -40))
+     133           0 :       LetterDesignator = 'H';
+     134         523 :     else if ((-40 > Lat) && (Lat >= -48))
+     135           0 :       LetterDesignator = 'G';
+     136         523 :     else if ((-48 > Lat) && (Lat >= -56))
+     137           0 :       LetterDesignator = 'F';
+     138         523 :     else if ((-56 > Lat) && (Lat >= -64))
+     139           0 :       LetterDesignator = 'E';
+     140         523 :     else if ((-64 > Lat) && (Lat >= -72))
+     141           0 :       LetterDesignator = 'D';
+     142         523 :     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         523 :       LetterDesignator = 'Z';
+     147      108663 :     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      108659 :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
+     160      108659 :     double a          = WGS84_A;
+     161      108659 :     double eccSquared = UTM_E2;
+     162      108659 :     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      108659 :     double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
+     170             : 
+     171      108659 :     double LatRad  = Lat * RADIANS_PER_DEGREE;
+     172      108659 :     double LongRad = LongTemp * RADIANS_PER_DEGREE;
+     173             :     double LongOriginRad;
+     174             :     int    ZoneNumber;
+     175             : 
+     176      108659 :     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      108659 :     if (ZoneNumber > 99)
+     180           0 :       ZoneNumber = 99;
+     181      108659 :     if (ZoneNumber < -9)
+     182           0 :       ZoneNumber = -9;
+     183             : 
+     184      108659 :     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+     185           0 :       ZoneNumber = 32;
+     186             : 
+     187             :     // Special zones for Svalbard
+     188      108659 :     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      108659 :     LongOrigin    = (ZoneNumber - 1) * 6 - 180 + 3;
+     200      108659 :     LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
+     201             : 
+     202             :     // compute the UTM Zone from the latitude and longitude
+     203      108659 :     snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+     204             : 
+     205      108403 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     206             : 
+     207      108403 :     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     208      108403 :     T = tan(LatRad) * tan(LatRad);
+     209      108403 :     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     210      108403 :     A = cos(LatRad) * (LongRad - LongOriginRad);
+     211             : 
+     212      108403 :     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
+     213      108403 :              (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
+     214      108403 :              (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
+     215      108403 :              (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+     216             : 
+     217      108403 :     UTMEasting =
+     218      108403 :         (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      108403 :     UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
+     221      108403 :                                          (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+     222      108403 :                                           (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     223      108403 :     if (Lat < 0)
+     224           0 :       UTMNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+     225      108403 :   }
+     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        1025 :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
+     246        1025 :     double                  k0         = UTM_K0;
+     247        1025 :     double                  a          = WGS84_A;
+     248        1025 :     double                  eccSquared = UTM_E2;
+     249             :     double                  eccPrimeSquared;
+     250        1025 :     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        1025 :     x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
+     261        1025 :     y = UTMNorthing;
+     262             : 
+     263        1025 :     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     264        1025 :     if ((*ZoneLetter - 'N') >= 0)
+     265        1025 :       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        1025 :     LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
+     272             : 
+     273        1025 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     274             : 
+     275        1025 :     M  = y / k0;
+     276        1025 :     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+     277             : 
+     278        1025 :     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        1025 :               (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     280        1025 :     phi1 = phi1Rad * DEGREES_PER_RADIAN;
+     281             : 
+     282        1025 :     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     283        1025 :     T1 = tan(phi1Rad) * tan(phi1Rad);
+     284        1025 :     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     285        1025 :     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     286        1025 :     D  = x / (N1 * k0);
+     287             : 
+     288        1025 :     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        1025 :                                                 (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     290        1025 :     Lat = Lat * DEGREES_PER_RADIAN;
+     291             : 
+     292        1025 :     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        1025 :            cos(phi1Rad);
+     294        1025 :     Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+     295        1025 :   }
+     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:34242680.3 %
Date:2024-01-07 21:47:14Functions:731131555.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
subscribe_handler.hpp +
75.8%75.8%
+ 		75.8 %91 / 12043.7 %437 / 1000
<unnamed>75.8 %91 / 12043.7 %437 / 1000
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
<unnamed>85.9 %55 / 6483.3 %40 / 48
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
<unnamed>64.3 %9 / 1487.5 %14 / 16
publisher_handler.hpp +
84.6%84.6%
+ 		84.6 %44 / 5296.5 %192 / 199
<unnamed>84.6 %44 / 5296.5 %192 / 199
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
<unnamed>92.9 %52 / 56100.0 %37 / 37
+
+
+ + + + +
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..11b028d41e --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html @@ -0,0 +1,218 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:34242680.3 %
Date:2024-01-07 21:47:14Functions:731131555.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
<unnamed>64.3 %9 / 1487.5 %14 / 16
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
subscribe_handler.hpp +
75.8%75.8%
+ 		75.8 %91 / 12043.7 %437 / 1000
<unnamed>75.8 %91 / 12043.7 %437 / 1000
publisher_handler.hpp +
84.6%84.6%
+ 		84.6 %44 / 5296.5 %192 / 199
<unnamed>84.6 %44 / 5296.5 %192 / 199
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
<unnamed>85.9 %55 / 6483.3 %40 / 48
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
<unnamed>92.9 %52 / 56100.0 %37 / 37
+
+
+ + + + +
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..277af85f9c --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail.html @@ -0,0 +1,218 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:34242680.3 %
Date:2024-01-07 21:47:14Functions:731131555.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
<unnamed>64.3 %9 / 1487.5 %14 / 16
publisher_handler.hpp +
84.6%84.6%
+ 		84.6 %44 / 5296.5 %192 / 199
<unnamed>84.6 %44 / 5296.5 %192 / 199
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
<unnamed>92.9 %52 / 56100.0 %37 / 37
subscribe_handler.hpp +
75.8%75.8%
+ 		75.8 %91 / 12043.7 %437 / 1000
<unnamed>75.8 %91 / 12043.7 %437 / 1000
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
<unnamed>85.9 %55 / 6483.3 %40 / 48
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
+
+
+ + + + +
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..305e7f324f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-f.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:34242680.3 %
Date:2024-01-07 21:47:14Functions:731131555.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
subscribe_handler.hpp +
75.8%75.8%
+ 		75.8 %91 / 12043.7 %437 / 1000
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
publisher_handler.hpp +
84.6%84.6%
+ 		84.6 %44 / 5296.5 %192 / 199
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
+
+
+ + + + +
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..84ab323e3e --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-l.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:34242680.3 %
Date:2024-01-07 21:47:14Functions:731131555.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
subscribe_handler.hpp +
75.8%75.8%
+ 		75.8 %91 / 12043.7 %437 / 1000
publisher_handler.hpp +
84.6%84.6%
+ 		84.6 %44 / 5296.5 %192 / 199
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
+
+
+ + + + +
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..fc5c1afabe --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:34242680.3 %
Date:2024-01-07 21:47:14Functions:731131555.6 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 1487.5 %14 / 16
publisher_handler.hpp +
84.6%84.6%
+ 		84.6 %44 / 5296.5 %192 / 199
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
subscribe_handler.hpp +
75.8%75.8%
+ 		75.8 %91 / 12043.7 %437 / 1000
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
+
+
+ + + + +
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..2537fdb9f2 --- /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-01-07 21:47:14Functions:141687.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const0
bool mrs_lib::ParamProvider::getParam<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const0
bool mrs_lib::ParamProvider::getParamImpl<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const1
bool mrs_lib::ParamProvider::getParam<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const1
bool mrs_lib::ParamProvider::getParamImpl<std::vector<double, std::allocator<double> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> >&) const1023
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> >&) const1023
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> > > >&) const2009
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> > > >&) const2009
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const3193
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const3193
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> >&) const9478
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> >&) const9478
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const11359
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const11359
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const23374
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const23374
+
+
+ + + +
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..c647f85c26 --- /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-01-07 21:47:14Functions:141687.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
bool mrs_lib::ParamProvider::getParamImpl<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&) const9478
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> > > >&) const2009
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> >&) const1023
bool mrs_lib::ParamProvider::getParamImpl<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const0
bool mrs_lib::ParamProvider::getParamImpl<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const1
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const11359
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const23374
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const3193
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> >&) const9478
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> > > >&) const2009
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> >&) const1023
bool mrs_lib::ParamProvider::getParam<std::vector<float, std::allocator<float> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<float, std::allocator<float> >&) const0
bool mrs_lib::ParamProvider::getParam<std::vector<int, std::allocator<int> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<int, std::allocator<int> >&) const1
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const11359
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const23374
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const3193
+
+
+ + + +
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..d0ec69873f --- /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-01-07 21:47:14Functions:141687.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef PARAM_PROVIDER_HPP
+       2             : #define PARAM_PROVIDER_HPP
+       3             : 
+       4             : #include <mrs_lib/param_provider.h>
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             :   template <typename T>
+       9       50437 :   bool ParamProvider::getParam(const std::string& param_name, T& value_out) const
+      10             :   {
+      11             :     try
+      12             :     {
+      13       50437 :       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       50437 :   bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const
+      24             :   {
+      25             :     {
+      26       50437 :       const auto found_node = findYamlNode(param_name);
+      27       50437 :       if (found_node.has_value())
+      28             :       {
+      29             :         try
+      30             :         {
+      31             :           // try catch is the only type-generic option...
+      32       43445 :           value_out = found_node.value().as<T>();
+      33       44153 :           return true;
+      34             :         }
+      35           0 :         catch (const YAML::BadConversion& e)
+      36             :         {}
+      37             :       }
+      38             : 
+      39             :     }
+      40             : 
+      41        6992 :     if (m_use_rosparam)
+      42        6992 :       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:445284.6 %
Date:2024-01-07 21:47:14Functions:19219996.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > > const&)2
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(mrs_msgs::Path_<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<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&)13
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)13
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&)13
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&)57
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)57
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)57
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&)57
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&)57
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&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)57
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&)57
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&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)57
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&)57
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&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)57
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&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)57
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&)57
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&)57
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)57
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&)58
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&)58
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&)60
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)60
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&)60
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)115
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&)118
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)118
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&)118
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&)167
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)167
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&)167
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&)171
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)171
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&)171
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&)227
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)227
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&)227
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&)228
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)228
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&)228
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)233
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&)286
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&)286
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&)332
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)332
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&)332
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&)342
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)342
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&)342
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&)454
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)454
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&)454
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)904
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)904
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)905
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)905
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)980
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)980
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1138
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1138
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)1186
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)1186
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1248
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1248
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)2856
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)2856
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6200
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6200
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)6601
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)6601
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)7115
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)7115
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)7900
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)7900
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)9212
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)9212
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)9599
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)9599
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)12964
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)12964
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)12964
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)12964
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)20127
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)20127
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)49554
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)49554
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)50929
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)50929
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)62216
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)62216
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)74351
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)74351
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)95404
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)96018
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)96018
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)117707
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)117724
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)135441
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)135441
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)198658
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)198661
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)207615
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)207725
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)270214
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)270540
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)388134
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)388391
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)454828
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)454842
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)785291
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)785542
+
+
+ + + +
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..1b32b136dc --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,876 @@ + + + + + + + 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:445284.6 %
Date:2024-01-07 21:47:14Functions:19219996.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)96018
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&)228
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)228
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)980
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&)57
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)117724
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&)60
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)60
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)20127
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&)342
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)342
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)57
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)6601
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)50929
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&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)198661
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&)332
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)332
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)62216
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&)57
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)388391
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&)227
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)227
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6200
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::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)1186
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&)57
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)7900
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&)57
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)785542
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&)454
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)454
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)270540
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&)167
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)167
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1248
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&)58
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)115
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&)49554
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)74351
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)9212
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&)57
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)12964
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&)57
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1138
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&)57
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)905
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&)57
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)9599
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
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&)57
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)114
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)904
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&)57
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)57
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&)57
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)207615
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&)118
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)118
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)454842
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)95404
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&)286
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)233
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)2856
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&)13
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)13
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)7115
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&)57
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)57
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&)12964
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&)57
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)57
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)135441
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&)171
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)171
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)96018
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&)228
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)980
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&)57
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)117707
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&)60
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)20127
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&)342
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::publish(mrs_msgs::BumperStatus_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)6601
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)50929
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)198658
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&)332
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)62216
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)388134
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&)227
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6200
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::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)1186
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)7900
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)785291
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&)454
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)270214
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&)167
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1248
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&)58
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&)49554
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)74351
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)9212
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)12964
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1138
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)905
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)9599
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)904
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&)57
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&)2
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&)57
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)207725
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&)118
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)454828
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&)286
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)2856
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&)13
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)7115
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&)57
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&)12964
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&)57
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)135441
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&)171
+
+
+ + + +
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..1ecbd7bc29 --- /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:445284.6 %
Date:2024-01-07 21:47:14Functions:19219996.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef PUBLISHER_HANDLER_HPP
+       2             : #define PUBLISHER_HANDLER_HPP
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             : 
+       7             : // --------------------------------------------------------------
+       8             : // |                    PublisherHandler_impl                   |
+       9             : // --------------------------------------------------------------
+      10             : 
+      11             : /* PublisherHandler_impl(void) //{ */
+      12             : 
+      13             : template <class TopicType>
+      14             : PublisherHandler_impl<TopicType>::PublisherHandler_impl(void) : publisher_initialized_(false) {
+      15             : }
+      16             : 
+      17             : //}
+      18             : 
+      19             : /* PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int &buffer_size, const bool &latch) //{ */
+      20             : 
+      21             : template <class TopicType>
+      22        3885 : PublisherHandler_impl<TopicType>::PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+      23        3885 :                                                         const double& rate) {
+      24             : 
+      25             :   {
+      26        3885 :     std::scoped_lock lock(mutex_publisher_);
+      27             : 
+      28        3885 :     publisher_ = nh.advertise<TopicType>(address, buffer_size, latch);
+      29             : 
+      30        3885 :     if (rate > 0.0) {
+      31             : 
+      32         400 :       throttle_ = true;
+      33             : 
+      34         400 :       throttle_min_dt_ = 1.0 / rate;
+      35             : 
+      36             :     } else {
+      37             : 
+      38        3485 :       throttle_ = false;
+      39             : 
+      40        3485 :       throttle_min_dt_ = 0;
+      41             :     }
+      42             : 
+      43        3885 :     last_time_published_ = ros::Time(0);
+      44             :   }
+      45             : 
+      46        3885 :   publisher_initialized_ = true;
+      47        3885 : }
+      48             : 
+      49             : //}
+      50             : 
+      51             : /* publish(TopicType& msg) //{ */
+      52             : 
+      53             : template <class TopicType>
+      54     3011384 : void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
+      55             : 
+      56     3011384 :   if (!publisher_initialized_) {
+      57           0 :     return;
+      58             :   }
+      59             : 
+      60             :   {
+      61     3010738 :     std::scoped_lock lock(mutex_publisher_);
+      62             : 
+      63     3010595 :     if (throttle_) {
+      64             : 
+      65      163111 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      66       72369 :         return;
+      67             :       }
+      68             : 
+      69       90744 :       last_time_published_ = ros::Time::now();
+      70             :     }
+      71             : 
+      72             :     try {
+      73     2938228 :       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             :     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           1 :         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        4343 : PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
+     169             : 
+     170        4286 :   if (this == &other) {
+     171           0 :     return *this;
+     172             :   }
+     173             : 
+     174        4343 :   if (other.impl_) {
+     175        4400 :     this->impl_ = other.impl_;
+     176             :   }
+     177             : 
+     178        4286 :   return *this;
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* copy constructor //{ */
+     184             : 
+     185             : template <class TopicType>
+     186       95404 : PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
+     187             : 
+     188       95404 :   if (other.impl_) {
+     189       95404 :     this->impl_ = other.impl_;
+     190             :   }
+     191       95404 : }
+     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        3885 : PublisherHandler<TopicType>::PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+     199        3885 :                                               const double& rate) {
+     200             : 
+     201        3885 :   impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(nh, address, buffer_size, latch, rate);
+     202        3885 : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* publish(const TopicType& msg) //{ */
+     207             : 
+     208             : template <class TopicType>
+     209     3012142 : void PublisherHandler<TopicType>::publish(const TopicType& msg) {
+     210             : 
+     211     3012142 :   impl_->publish(msg);
+     212     3012754 : }
+     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             : }
+     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..6040f866266270d282ed333f47d67fce552a18f5 GIT binary patch literal 799 zcmV+)1K|9LP)WdBtP7%&hFFN5RKR&B3IPI(DQh)13{=Zt${)1knM zuy6}m#p+=!F+KC5uw60sH(fm=BcAnf=5uQtHW-`6nqpxoz7DR)On1m!W|$5FbAdH4 z33lN)u7C*jGc5&KSGk%$DVL&RDEOw)2An6mWDsG~7Ey>~rE)hzM-&E7g`lDcBN`{A z4r^CLyK=qwRz#IU9`>Jh308gJUDK)ykyMK1)CH?17w%qbdDEC>is>~reTWEJCItM- z{>ZwVGNk^P_K2t#sWKJB@GLQ+I8=N*Ua!~lQI+WdC>cduZf(t?92h!c5OOJ#j_=<~ zgSb|@twV&i4Ib|$*D5lByr4;GLIb5c^0CmJaUXSsKh>_`D09wytg?fg%TuO{`q4;M(U5YEq?NTZ7uSG zP>Ig1-0YnXH~BbR^<*Ubq{7EO_~`N{6 + + + + + + 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-01-07 21:47:14Functions:3737100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&, int const&, double const&)2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::asyncRun()2
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int const&)2
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)10
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)10
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)57
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)82
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)116
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)116
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)116
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)225
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)225
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)226
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)226
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)344
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)344
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)344
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)455
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)455
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)614
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)614
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)671
+
+
+ + + +
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..66e9d1716e --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-01-07 21:47:14Functions:3737100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)82
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)10
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)226
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)116
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)116
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)225
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)344
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)344
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)455
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&)614
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)614
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)10
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)226
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)116
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)225
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)344
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)455
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&)671
+
+
+ + + +
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..02f54e9c99 --- /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-01-07 21:47:14Functions:3737100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::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::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::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::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&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::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::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::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::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() 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::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::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::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::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::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::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::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::start()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().22
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::start()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl().22
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const2
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()3
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().23
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()3
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const3
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const3
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const3
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const3
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const3
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::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::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()13
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().213
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()15
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&)15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().215
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&)15
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()16
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().216
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const16
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().218
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const20
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const20
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const54
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const54
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const54
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const54
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&)57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const58
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const58
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const58
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()60
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().260
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()60
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()60
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const60
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const60
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const60
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()61
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()61
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const65
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()73
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()73
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const73
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const73
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const74
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const74
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()101
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&)101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()101
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&)101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const101
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const108
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const108
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()114
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&)114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()114
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&)114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()114
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&)114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2114
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()114
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()114
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const114
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()117
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&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()117
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&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2117
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()117
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const117
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const121
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const121
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()127
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&)127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2127
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&)127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()166
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const166
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()169
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const169
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()170
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const170
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()171
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()171
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const171
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()177
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&)177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()177
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&)177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const178
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()184
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&)184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()184
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&)184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const184
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()186
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2186
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()189
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2189
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)197
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)197
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()224
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&)224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()224
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&)224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()224
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()224
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const224
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()228
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&)228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2228
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&)228
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const229
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()234
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2234
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const234
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()243
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2243
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()246
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2246
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const246
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const254
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const270
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const270
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()348
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&)348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()348
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&)348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2348
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()361
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()361
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const361
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const361
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)373
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)376
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const460
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const538
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)904
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)904
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)904
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)904
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)1075
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)1078
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()1344
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()1344
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1344
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const1344
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const1809
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const1809
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)2533
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)2542
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()2869
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const2870
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()2871
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const2872
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const4316
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const4318
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6386
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()7840
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const7840
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const8195
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const12401
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const12401
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)12573
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)12573
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)12695
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)12716
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)15495
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)15495
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const29534
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const29534
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()29589
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const29591
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()29593
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const29595
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)30849
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)30871
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()34308
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const34323
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()34329
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const34331
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const36615
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const36619
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)38301
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)38429
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const38890
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const40704
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)62199
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)62199
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)74333
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)74333
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)97516
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)97545
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()99536
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()99536
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const99536
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const99536
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const99537
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const99537
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const109652
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()109656
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()109682
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const109682
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const110158
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const111969
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const112286
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()112329
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()112345
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const112353
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)115939
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)115946
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const127920
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const127920
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const147006
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const156659
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)168614
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)168668
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const174349
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const174353
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)186792
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)186815
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)188828
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)188860
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const207677
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const207739
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)208791
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)209213
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)212994
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)213087
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)215027
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)215079
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const231979
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const232045
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const242792
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()242949
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const242986
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()243037
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()266696
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()266704
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const266704
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const266704
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)304216
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)304304
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const317648
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const317799
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)329970
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)330008
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const430744
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const430772
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()430773
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()430774
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)493515
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)493535
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const551359
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const551424
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)646633
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)646680
+
+
+ + + +
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..4caf59a8cc --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,4080 @@ + + + + + + + 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:9112075.8 %
Date:2024-01-07 21:47:14Functions:437100043.7 %
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&)213087
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()224
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&)224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2224
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&)212994
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()224
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&)224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2224
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()114
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&)114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2114
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()114
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&)114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2114
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)168668
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()2869
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&)101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2101
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&)168614
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()101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()2871
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&)101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2101
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&)97545
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()61
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()1344
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&)57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)97516
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()61
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()1344
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&)57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)330008
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()112345
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&)184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2184
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&)329970
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()184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()112329
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&)184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2184
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)646633
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()430774
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&)348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2348
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&)646680
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&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()430773
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&)348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2348
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)493535
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()166
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&)127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2127
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&)493515
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()130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()169
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&)127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)215027
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()99536
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&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2117
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&)215079
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()117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()99536
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&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2117
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)209213
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()243037
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2224
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&)208791
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()224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()242949
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2224
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)186815
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()29589
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&)228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2228
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&)186792
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()234
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()29593
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&)228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2234
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)62199
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()361
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)62199
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()60
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()361
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)115946
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().260
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&)115939
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()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)15495
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()538
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)15495
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()57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()538
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)2533
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()170
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2171
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&)2542
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()171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()171
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2171
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&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()3
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().23
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()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)74333
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()3
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)74333
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()57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()3
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)38429
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()34308
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2243
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&)38301
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()246
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()34329
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2246
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)197
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()13
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().213
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&)197
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()16
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().216
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)904
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)904
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()60
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)1078
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().215
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&)1075
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()18
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)> const&)15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().218
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().257
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)30871
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()109656
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2186
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&)30849
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()189
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()109682
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2189
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)904
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().257
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&)904
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()60
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().260
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)12716
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()73
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&)12695
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()73
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&)188828
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()114
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&)114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2114
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&)188860
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()117
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()7840
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&)114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2117
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)304216
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()266696
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&)177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2177
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&)304304
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()177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()266704
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&)177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2177
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)376
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::start()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().22
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const> const&)373
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::start()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::~Impl().22
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)12573
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()21897
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2114
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&)12573
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()114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()21897
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2114
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]() const224
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]() const114
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const4318
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const2870
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() const4316
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const2872
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]() const101
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() const12401
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1344
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() const12401
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const1344
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]() const58
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const232045
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const112286
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() const231979
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const112353
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]() const184
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const551424
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const430744
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]() const58
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const551359
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const430772
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]() const460
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const108
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const121
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const166
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() const108
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const121
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const169
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]() const130
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const99537
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const99536
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() const99537
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const99536
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]() const117
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const207677
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const317799
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const242792
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() const207739
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const317648
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const242986
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]() const224
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const36615
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const29591
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() const36619
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const29595
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]() const234
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const3
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const361
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() const3
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const361
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]() const60
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]() const60
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const538
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() const538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const538
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]() const57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const270
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const171
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]() const58
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const270
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const170
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]() const229
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() const0
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]() const3
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]() const57
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]() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const3
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() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const3
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]() const57
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const38890
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const34323
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() const40704
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const34331
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]() const246
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6386
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() const8195
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]() const16
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const54
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() const54
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const1809
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]() const60
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const20
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const20
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const1
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const18
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const57
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const110158
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const109652
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]() const65
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const111969
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const109682
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]() const254
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const54
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() const54
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const1809
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]() const60
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const74
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const73
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() const74
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const73
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() const127920
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const147006
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() const127920
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const156659
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const7840
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]() const117
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const174349
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const266704
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() const174353
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const266704
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]() const178
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const2
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const29534
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const21897
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() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const29534
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const21897
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]() const114
+
+
+ + + +
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..749638efab --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html @@ -0,0 +1,413 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9112075.8 %
Date:2024-01-07 21:47:14Functions:437100043.7 %
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        3398 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29        3398 :         : m_nh(options.nh),
+      30        3398 :           m_topic_name(options.topic_name),
+      31        3398 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35        3398 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39        3398 :           m_queue_size(options.queue_size),
+      40        3398 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42        3398 :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      43             :       {
+      44             :         // initialize a new TimeoutManager if not provided by the user
+      45          55 :         if (!m_timeout_manager)
+      46          55 :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(2.0/options.no_message_timeout.toSec()));
+      47             : 
+      48             :         // initialize the callback for the TimeoutManager
+      49         110 :         std::function<void(const ros::Time&)> timeout_mgr_callback;
+      50          55 :         if (options.timeout_callback)
+      51           1 :           timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      52             :         else
+      53          54 :           timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      54             : 
+      55             :         // register the timeout callback with the TimeoutManager
+      56          55 :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, timeout_mgr_callback);
+      57             :       }
+      58             : 
+      59       10194 :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      60        3398 :       if (m_node_name.empty())
+      61           1 :         ROS_INFO_STREAM(msg);
+      62             :       else
+      63        3397 :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      64        3398 :     }
+      65             :     //}
+      66             : 
+      67        3539 :     virtual ~Impl() = default;
+      68             : 
+      69             :   public:
+      70             :     /* getMsg() method //{ */
+      71     1361163 :     virtual typename MessageType::ConstPtr getMsg()
+      72             :     {
+      73     1361163 :       m_new_data = false;
+      74     1361163 :       m_used_data = true;
+      75     1361163 :       return peekMsg();
+      76             :     }
+      77             :     //}
+      78             : 
+      79             :     /* peekMsg() method //{ */
+      80     1361227 :     virtual typename MessageType::ConstPtr peekMsg() const
+      81             :     {
+      82             :       /* assert(m_got_data); */
+      83             :       /* if (!m_got_data) */
+      84             :       /*   ROS_ERROR("[%s]: No data received yet from topic '%s' (forgot to check hasMsg()?)! Returning empty message.", m_node_name.c_str(), */
+      85             :       /*             topicName().c_str()); */
+      86     1361227 :       return m_latest_message;
+      87             :     }
+      88             :     //}
+      89             : 
+      90             :     /* hasMsg() method //{ */
+      91     1462326 :     virtual bool hasMsg() const
+      92             :     {
+      93     1462326 :       return m_got_data;
+      94             :     }
+      95             :     //}
+      96             : 
+      97             :     /* newMsg() method //{ */
+      98      317648 :     virtual bool newMsg() const
+      99             :     {
+     100      317648 :       return m_new_data;
+     101             :     }
+     102             :     //}
+     103             : 
+     104             :     /* usedMsg() method //{ */
+     105           0 :     virtual bool usedMsg() const
+     106             :     {
+     107           0 :       return m_used_data;
+     108             :     }
+     109             :     //}
+     110             : 
+     111             :     /* waitForNew() method //{ */
+     112           0 :     virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout)
+     113             :     {
+     114             :       // convert the ros type to chrono type
+     115           0 :       const std::chrono::duration<float> chrono_timeout(timeout.toSec());
+     116             :       // lock the mutex guarding the m_new_data flag
+     117           0 :       std::unique_lock lock(m_new_data_mtx);
+     118             :       // if new data is available, return immediately, otherwise attempt to wait for new data using the respective condition variable
+     119           0 :       if (m_new_data)
+     120           0 :         return getMsg();
+     121           0 :       else if (m_new_data_cv.wait_for(lock, chrono_timeout) == std::cv_status::no_timeout && m_new_data)
+     122           0 :         return getMsg();
+     123             :       else
+     124           0 :         return nullptr;
+     125             :     };
+     126             :     //}
+     127             : 
+     128             :     /* lastMsgTime() method //{ */
+     129      357777 :     virtual ros::Time lastMsgTime() const
+     130             :     {
+     131      357777 :       return m_latest_message_time;
+     132             :     };
+     133             :     //}
+     134             : 
+     135             :     /* topicName() method //{ */
+     136        3671 :     virtual std::string topicName() const
+     137             :     {
+     138        3671 :       std::string ret = m_sub.getTopic();
+     139        3671 :       if (ret.empty())
+     140        3489 :         ret = m_nh.resolveName(m_topic_name);
+     141        3671 :       return ret;
+     142             :     }
+     143             :     //}
+     144             : 
+     145             :     /* start() method //{ */
+     146        3438 :     virtual void start()
+     147             :     {
+     148        3438 :       if (m_timeout_manager)
+     149          59 :         m_timeout_manager->start(m_timeout_id);
+     150        3438 :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     151        3438 :     }
+     152             :     //}
+     153             : 
+     154             :     /* stop() method //{ */
+     155           5 :     virtual void stop()
+     156             :     {
+     157           5 :       if (m_timeout_manager)
+     158           5 :         m_timeout_manager->pause(m_timeout_id);
+     159           5 :       m_sub.shutdown();
+     160           5 :     }
+     161             :     //}
+     162             : 
+     163             :   protected:
+     164             :     ros::NodeHandle m_nh;
+     165             :     ros::Subscriber m_sub;
+     166             : 
+     167             :   protected:
+     168             :     std::string m_topic_name;
+     169             :     std::string m_node_name;
+     170             : 
+     171             :   protected:
+     172             :     bool m_got_data;   // whether any data was received
+     173             : 
+     174             :     mutable std::mutex m_new_data_mtx;
+     175             :     mutable std::condition_variable m_new_data_cv;
+     176             :     bool m_new_data;   // whether new data was received since last call to get_data
+     177             : 
+     178             :     bool m_used_data;  // whether get_data was successfully called at least once
+     179             : 
+     180             :   protected:
+     181             :     std::shared_ptr<mrs_lib::TimeoutManager> m_timeout_manager;
+     182             :     mrs_lib::TimeoutManager::timeout_id_t m_timeout_id;
+     183             : 
+     184             :   protected:
+     185             :     ros::Time m_latest_message_time;
+     186             :     typename MessageType::ConstPtr m_latest_message;
+     187             :     message_callback_t m_message_callback;
+     188             : 
+     189             :   private:
+     190             :     uint32_t m_queue_size;
+     191             :     ros::TransportHints m_transport_hints;
+     192             : 
+     193             :   protected:
+     194             :     /* default_timeout_callback() method //{ */
+     195           0 :     void default_timeout_callback(const std::string& topic_name, const ros::Time& last_msg)
+     196             :     {
+     197           0 :       const ros::Duration since_msg = (ros::Time::now() - last_msg);
+     198           0 :       const auto n_pubs = m_sub.getNumPublishers();
+     199           0 :       const std::string txt = "Did not receive any message from topic '" + topic_name + "' for " + std::to_string(since_msg.toSec()) + "s ("
+     200             :                               + std::to_string(n_pubs) + " publishers on this topic)";
+     201           0 :       if (m_node_name.empty())
+     202           0 :         ROS_WARN_STREAM(txt);
+     203             :       else
+     204           0 :         ROS_WARN_STREAM("[" << m_node_name << "]: " << txt);
+     205           0 :     }
+     206             :     //}
+     207             : 
+     208             :     /* process_new_message() method //{ */
+     209     3421494 :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     210             :     {
+     211     3421494 :       m_latest_message_time = ros::Time::now();
+     212     3423334 :       m_latest_message = msg;
+     213             :       // If the message callback is registered, the new data will immediately be processed,
+     214             :       // so reset the flag. Otherwise, set the flag.
+     215     3421328 :       m_new_data = !m_message_callback;
+     216     3418760 :       m_got_data = true;
+     217     3418760 :       m_new_data_cv.notify_one();
+     218     3422037 :     }
+     219             :     //}
+     220             : 
+     221             :     /* data_callback() method //{ */
+     222           0 :     virtual void data_callback(const typename MessageType::ConstPtr& msg)
+     223             :     {
+     224             :       {
+     225           0 :         std::lock_guard lck(m_new_data_mtx);
+     226           0 :         if (m_timeout_manager)
+     227           0 :           m_timeout_manager->reset(m_timeout_id);
+     228           0 :         process_new_message(msg);
+     229             :       }
+     230             : 
+     231             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     232           0 :       if (m_message_callback)
+     233           0 :         m_message_callback(msg);
+     234           0 :     }
+     235             :     //}
+     236             :   };
+     237             :   //}
+     238             : 
+     239             :   /* SubscribeHandler_threadsafe class //{ */
+     240             :   template <typename MessageType>
+     241             :   class SubscribeHandler<MessageType>::ImplThreadsafe : public SubscribeHandler<MessageType>::Impl
+     242             :   {
+     243             :   private:
+     244             :     using impl_class_t = SubscribeHandler<MessageType>::Impl;
+     245             : 
+     246             :   public:
+     247             :     using timeout_callback_t = typename impl_class_t::timeout_callback_t;
+     248             :     using message_callback_t = typename impl_class_t::message_callback_t;
+     249             : 
+     250             :     friend class SubscribeHandler<MessageType>;
+     251             : 
+     252             :   public:
+     253        3398 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     254        3398 :         : impl_class_t::Impl(options, message_callback)
+     255             :     {
+     256        3398 :     }
+     257             : 
+     258             :   public:
+     259     1443746 :     virtual bool hasMsg() const override
+     260             :     {
+     261     2887163 :       std::lock_guard lck(m_mtx);
+     262     2887217 :       return impl_class_t::hasMsg();
+     263             :     }
+     264      317799 :     virtual bool newMsg() const override
+     265             :     {
+     266      634889 :       std::lock_guard lck(m_mtx);
+     267      634733 :       return impl_class_t::newMsg();
+     268             :     }
+     269           0 :     virtual bool usedMsg() const override
+     270             :     {
+     271           0 :       std::lock_guard lck(m_mtx);
+     272           0 :       return impl_class_t::usedMsg();
+     273             :     }
+     274     1353363 :     virtual typename MessageType::ConstPtr getMsg() override
+     275             :     {
+     276     2706418 :       std::lock_guard lck(m_mtx);
+     277     2705857 :       return impl_class_t::getMsg();
+     278             :     }
+     279     1353052 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     280             :     {
+     281     2705858 :       std::lock_guard lck(m_mtx);
+     282     2705612 :       return impl_class_t::peekMsg();
+     283             :     }
+     284      357715 :     virtual ros::Time lastMsgTime() const override
+     285             :     {
+     286      715419 :       std::lock_guard lck(m_mtx);
+     287      715352 :       return impl_class_t::lastMsgTime();
+     288             :     };
+     289         182 :     virtual std::string topicName() const override
+     290             :     {
+     291         364 :       std::lock_guard lck(m_mtx);
+     292         364 :       return impl_class_t::topicName();
+     293             :     };
+     294        3402 :     virtual void start() override
+     295             :     {
+     296        6804 :       std::lock_guard lck(m_mtx);
+     297        6804 :       return impl_class_t::start();
+     298             :     }
+     299           5 :     virtual void stop() override
+     300             :     {
+     301          10 :       std::lock_guard lck(m_mtx);
+     302          10 :       return impl_class_t::stop();
+     303             :     }
+     304             : 
+     305        6796 :     virtual ~ImplThreadsafe() override = default;
+     306             : 
+     307             :   protected:
+     308     3422129 :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     309             :     {
+     310             :       {
+     311     6841244 :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     312     3410932 :         if (this->m_timeout_manager)
+     313       94931 :           this->m_timeout_manager->reset(this->m_timeout_id);
+     314     3397964 :         impl_class_t::process_new_message(msg);
+     315             :       }
+     316             : 
+     317             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     318     3412371 :       if (this->m_message_callback)
+     319     1921404 :         impl_class_t::m_message_callback(msg);
+     320     3407621 :     }
+     321             : 
+     322             :   private:
+     323             :     mutable std::recursive_mutex m_mtx;
+     324             :   };
+     325             :   //}
+     326             : 
+     327             : }  // namespace mrs_lib
+     328             : 
+     329             : #endif  // SUBSCRIBE_HANDLER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html new file mode 100644 index 0000000000..02a4b3f18a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html @@ -0,0 +1,103 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c6682e9dff97067abef33079f6788709fa7e9ee7 GIT binary patch literal 1386 zcmV-w1(o`VP)~QyZhEahLIV1j%{))D8;>z# zP6X|k1^_jztv+xFfD(#I)}xe}B(&trpm8SqXQJ$5JS6zX1W`;R5P}A%Z8~}*UvpLx zYa=%^wvC>{m9@gn_fW1!39MW zI#;M{EvgnMBI%VjVY0^AX~Lub&RPTBBz3IywmsWp^AWCLszV3S^qkoAh>bt@yB zw`0$J5h!s?L2(OGtWMmum|WuN%4Kc&t$; zh;acA>fcJ>>kRZx?iD%3@0q6?KhZo{-7~3-`@W*BJ+5TasxQFF;_ufukNtM_^c7Ze0zX7LZ49%-60d+El z+SPzPJ5bI}R7J_?q6=I!Qm;L+KuIVKlM%Xx1;UL2@`W1mVW7(?0Iq!?io_8z3Js19 z&BztU752@#evhAfsO&YJ<^f0@sc#5R1#pLuM%Mx<9uDezsrczStbGsN2N;m(zVESD zIberGG+Ti6ixjMqQD~k_F}6;-(+f5F8iRNGya?398J>bvb!ZC!bn>vK(mnot(^dfR z5P+`PA#ml2Y+v_mevjRu)2W58ui{_5+To%=T5n8a9azBbHCO*2pj>OFoVves?M1|+ z*t|-q1Wm@cMIzswk*!c*ashtLIfF6)H@RLg1CXiUQQ|1oEO7XA&6`(q3f>UL29!Zq zX+E1FWfQ{J%J>CcRDt<&6@}87h8E~Z0u+onxLvHR1 z&pw(55Vt8qH5E!_)+dUp7VzuJ(5F)R)XZL#`Uzz|W-T5d9^=E>a;0uz7Zs2&di-eg z_D@&N$hDT$VHORUBCe&@c*+p07f7-iTS1XS0O0WcTG+)^O4F08sZ0y_jrvi(nF@-2 sL)M-5^@V;*U-sWqWWyQ<_9ztY4_tc183Gfly#N3J07*qoM6N<$f-2dM6aWAK literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html new file mode 100644 index 0000000000..c199f7356d --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-01-07 21:47:14Functions: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..9a13c7110d --- /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-01-07 21:47:14Functions: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..0331602be8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html @@ -0,0 +1,179 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3a8f0b8378ef00e8147fa63b03c2ae9387250e7f GIT binary patch literal 527 zcmV+q0`UEbP)JN zR7i={R@)VWAPglNzzVVfuHXoD|5d7xK!QgI7C)M{>Va!+9wd!%&RG}5nx?3MYH2mKoS>NR(V4pexZ8zpDW;uZNe9!VlV2nV^fG~`kRhm|F zi$qoux6G_F!QC;67GAA$~bb?j*aS0!Or+(;H2D4Zlb5g7yb)bo^6@Po)e+E zv2lhZ4>NI^h(pUNMg=cMosb3(d@@TYYJ^UXJO?E8l=8wj^Ky6s2aEfFGZ-&6?s*Z@ zkb!CL3)RL&8l}blAEWEhYu&%6 literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html new file mode 100644 index 0000000000..9b63c27544 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html @@ -0,0 +1,272 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-01-07 21:47:14Functions:404883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::doTransform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)0
geometry_msgs::Quaternion_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Vector3Stamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Point_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
cv::Point3_<double> mrs_lib::Transformer::copyChangeFrame<cv::Point3_<double> >(cv::Point3_<double> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
Eigen::Quaternion<double, 0> mrs_lib::Transformer::copyChangeFrame<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)0
std::optional<cv::Point3_<double> > mrs_lib::Transformer::doTransform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transformImpl<cv::Point3_<double> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, cv::Point3_<double> const&)1
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<cv::Point3_<double> > mrs_lib::Transformer::transform<cv::Point3_<double> >(cv::Point3_<double> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)1
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transformImpl<Eigen::Quaternion<double, 0> >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, Eigen::Quaternion<double, 0> const&)2
std::optional<Eigen::Quaternion<double, 0> > mrs_lib::Transformer::transform<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)2
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)4
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)5
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)6
std::optional<Eigen::Matrix<double, 3, 1, 0, 3, 1> > mrs_lib::Transformer::transform<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Point_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Point_<std::allocator<void> > const&)16
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)18
std::optional<geometry_msgs::Vector3_<std::allocator<void> > > mrs_lib::Transformer::transformImpl<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::TransformStamped_<std::allocator<void> > const&, geometry_msgs::Vector3_<std::allocator<void> > const&)18
std::optional<geometry_msgs::Vector3Stamped_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)468
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&)6857
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&)6857
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)6857
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&)14617
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)14617
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&)47908
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&)56821
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&)93958
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)93958
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&)161878
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&)176476
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&)176480
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&)176495
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)191090
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&)453845
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&)453946
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)453984
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&)454455
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&)454455
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&)464895
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&)465003
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&)476213
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)558915
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&)570168
+
+
+ + + +
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..59430fdba2 --- /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-01-07 21:47:14Functions:404883.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)6
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::doTransform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)476213
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&)161878
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&)454455
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&)570168
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&)176495
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&)454455
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&)93958
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&)14617
geometry_msgs::Vector3Stamped_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Point_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Point_<std::allocator<void> > >(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
geometry_msgs::Vector3_<std::allocator<void> > mrs_lib::Transformer::copyChangeFrame<geometry_msgs::Vector3_<std::allocator<void> > >(geometry_msgs::Vector3_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
cv::Point3_<double> mrs_lib::Transformer::copyChangeFrame<cv::Point3_<double> >(cv::Point3_<double> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
Eigen::Quaternion<double, 0> mrs_lib::Transformer::copyChangeFrame<Eigen::Quaternion<double, 0> >(Eigen::Quaternion<double, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::Quaternion_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)1
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transformSingle<geometry_msgs::PoseStamped_<std::allocator<void> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)465003
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&)464895
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&)176480
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&)176476
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&)453946
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&)453845
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&)6857
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&)6857
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)558915
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)191090
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)453984
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)6857
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)93958
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)14617
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)0
std::optional<geometry_msgs::Quaternion_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::Quaternion_<std::allocator<void> > >(geometry_msgs::Quaternion_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)5
std::optional<geometry_msgs::PoseStamped_<std::allocator<void> > > mrs_lib::Transformer::transform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)47908
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&)468
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&)56821
+
+
+ + + +
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..10b8192509 --- /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-01-07 21:47:14Functions:404883.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef TRANSFORMER_HPP
+       2             : #define TRANSFORMER_HPP
+       3             : 
+       4             : // clang: MatousFormat
+       5             : 
+       6             : namespace mrs_lib
+       7             : {
+       8             : 
+       9             :   // | --------------------- helper methods --------------------- |
+      10             : 
+      11             :   /* getHeader() overloads for different message types (pointers, pointclouds etc) //{ */
+      12             : 
+      13             :   template <typename msg_t>
+      14     1210846 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16     1210846 :     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      108575 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34      108575 :     msg.header = header;
+      35      108575 :   }
+      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      108575 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50      108575 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53      217150 :       std_msgs::Header new_header = getHeader(what);
+      54      108575 :       new_header.frame_id = frame_id;
+      55      108575 :       setHeader(ret, new_header);
+      56             :     }
+      57      108575 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65     1201161 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67     2402296 :     const std::string from_frame = frame_from(tf);
+      68     2402271 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70     1201147 :     if (from_frame == to_frame)
+      71      108575 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73     2185159 :     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      638096 :       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      638095 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90        2048 :         const std::optional<T> tmp = doTransform(what, tf);
+      91        1025 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93        1025 :         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      454482 :       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     1091553 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115     1092587 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119     2185069 :       T result;
+     120     1092570 :       tf2::doTransform(what, result, tf);
+     121     1092556 :       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     1102073 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140     2204657 :     const std_msgs::Header orig_header = getHeader(what);
+     141     2204236 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145     1102287 :   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     2204866 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149     1102598 :     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     2205169 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156     2205147 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157     2205130 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160     2205153 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161     1102595 :     if (!tf_opt.has_value())
+     162        6677 :       return std::nullopt;
+     163     1095891 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166     2191793 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167     1095907 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175      105215 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177      210430 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179      105215 :     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      210430 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186      210430 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187      210430 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189      105215 :     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..b1e92095ab --- /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-01-07 21:47:14Functions: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..52b4552dd1 --- /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-01-07 21:47:14Functions: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..e8f12332dd --- /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-01-07 21:47:14Functions:81266.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1d33d5b3039a981579bb4f50a9c00a2c8709ad9b GIT binary patch literal 1198 zcmV;f1X25mP)Y@{;Pb0B(89X1lp$WlRn3FWZf6Cw_WVJ+}+ydtQ`=_ zd4PPgbS9hXQz+oPx)ki&7 z-lQE#rEk)z9uGELd87|74GK~T#|W?+u3XZe6t2^vl^w1d^jri}M|c;c3rf)C56QZq zP>s|XTiZ;bsJ#Vv-hdqz_$BTdbHN)i=DQZ)l5v>`+*`m1a!FMX_`s(SmpO- zp$vyfR|C*SHhH6}0WwL`B(6KO!^))|7!eAY$hGhdq-(Utn_=63OFES_bj!U50C8YR z|N7J=_cr$>VLl@P%-P){#tAJ~{L6R_w;t2FXAF>2UxyPws4?ZRe#OISm*?A}%rGNq zZ^^7Vo9;iJkq`B_Y=@kl0KzY}Aw>xP^-|D;VL_|NIq#{C2ogQcHsOZP_(abshj)TrRs^3 zRDJ%qOg&hws;@$&LlQi$D@$mg=^NrpvL6ojS`Pb^=MJ& zI<$0_Mxku)5@`mY@u80@ds!ioK4>QJ-s>hI8lqjf%z4bxSXJQzPkg@O>F7ldYVaim ze6B)6>E3j9;8EqM3Hx9J`$AVSoejBw%?C7ptCNz#zfL;|HFrYu4`1RPbHQ`N;s5{u M07*qoM6N<$f)tiG*8l(j literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/index-detail-sort-f.html b/mrs_lib/include/mrs_lib/index-detail-sort-f.html new file mode 100644 index 0000000000..deac107a41 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-f.html @@ -0,0 +1,426 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:74896377.7 %
Date:2024-01-07 21:47:14Functions:759127959.3 %
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 / 11135.7 %20 / 56
<unnamed>90.1 %100 / 11135.7 %20 / 56
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.2 %356 / 709
<unnamed>79.6 %39 / 4950.2 %356 / 709
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
<unnamed>81.5 %44 / 5461.0 %25 / 41
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
<unnamed>79.3 %23 / 2975.0 %9 / 12
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
mutex.h +
100.0%
+ 		100.0 %11 / 1182.7 %67 / 81
<unnamed>100.0 %11 / 1182.7 %67 / 81
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
<unnamed>88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %116 / 117
<unnamed>100.0 %4 / 499.1 %116 / 117
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 %20 / 20
<unnamed>100.0 %3 / 3100.0 %20 / 20
+
+
+ + + + +
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..e1fc4b73b7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-l.html @@ -0,0 +1,426 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:74896377.7 %
Date:2024-01-07 21:47:14Functions:759127959.3 %
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
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
<unnamed>79.3 %23 / 2975.0 %9 / 12
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.2 %356 / 709
<unnamed>79.6 %39 / 4950.2 %356 / 709
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
<unnamed>81.5 %44 / 5461.0 %25 / 41
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
<unnamed>88.3 %248 / 28197.2 %70 / 72
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
<unnamed>90.1 %100 / 11135.7 %20 / 56
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %20 / 20
<unnamed>100.0 %3 / 3100.0 %20 / 20
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %116 / 117
<unnamed>100.0 %4 / 499.1 %116 / 117
mutex.h +
100.0%
+ 		100.0 %11 / 1182.7 %67 / 81
<unnamed>100.0 %11 / 1182.7 %67 / 81
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
<unnamed>100.0 %15 / 15100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index-detail.html b/mrs_lib/include/mrs_lib/index-detail.html new file mode 100644 index 0000000000..0298b24f72 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail.html @@ -0,0 +1,426 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:74896377.7 %
Date:2024-01-07 21:47:14Functions:759127959.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
<unnamed>79.3 %23 / 2975.0 %9 / 12
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
<unnamed>65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
<unnamed>76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
<unnamed>81.5 %44 / 5461.0 %25 / 41
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
<unnamed>33.3 %33 / 9934.1 %15 / 44
mutex.h +
100.0%
+ 		100.0 %11 / 1182.7 %67 / 81
<unnamed>100.0 %11 / 1182.7 %67 / 81
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
<unnamed>88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %116 / 117
<unnamed>100.0 %4 / 499.1 %116 / 117
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
<unnamed>90.1 %100 / 11135.7 %20 / 56
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
<unnamed>33.3 %2 / 633.3 %1 / 3
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %20 / 20
<unnamed>100.0 %3 / 3100.0 %20 / 20
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.2 %356 / 709
<unnamed>79.6 %39 / 4950.2 %356 / 709
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
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..12f0f6a204 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-f.html @@ -0,0 +1,282 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:74896377.7 %
Date:2024-01-07 21:47:14Functions:759127959.3 %
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 / 11135.7 %20 / 56
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.2 %356 / 709
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
mutex.h +
100.0%
+ 		100.0 %11 / 1182.7 %67 / 81
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %116 / 117
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 %20 / 20
+
+
+ + + + +
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..2a0909076a --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-l.html @@ -0,0 +1,282 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:74896377.7 %
Date:2024-01-07 21:47:14Functions:759127959.3 %
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
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.2 %356 / 709
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %20 / 20
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %116 / 117
mutex.h +
100.0%
+ 		100.0 %11 / 1182.7 %67 / 81
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/index.html b/mrs_lib/include/mrs_lib/index.html new file mode 100644 index 0000000000..6850a87479 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index.html @@ -0,0 +1,282 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:74896377.7 %
Date:2024-01-07 21:47:14Functions:759127959.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
attitude_converter.h +
79.3%79.3%
+ 		79.3 %23 / 2975.0 %9 / 12
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5461.0 %25 / 41
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
mutex.h +
100.0%
+ 		100.0 %11 / 1182.7 %67 / 81
param_loader.h +
88.3%88.3%
+ 		88.3 %248 / 28197.2 %70 / 72
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.1 %116 / 117
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11135.7 %20 / 56
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %20 / 20
subscribe_handler.h +
79.6%79.6%
+ 		79.6 %39 / 4950.2 %356 / 709
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
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..d39e4e9aec --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-01-07 21:47:14Functions:254161.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::LKF<2, 1, 1>::LKF(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)0
std::enable_if<(1)!=(0), Eigen::Matrix<double, 4, 1, 0, 4, 1> >::type mrs_lib::LKF<4, 1, 2>::state_predict<1>(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)0
mrs_lib::LKF<4, 1, 2>::covariance_predict(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double)0
mrs_lib::LKF<4, 1, 2>::correction_optimized(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)0
mrs_lib::LKF<4, 1, 2>::LKF(Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&)0
mrs_lib::LKF<4, 1, 2>::LKF()0
mrs_lib::LKF<2, 1, 1>::inverse_exception::what() const0
mrs_lib::LKF<2, 1, 1>::predict(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, double) const0
mrs_lib::LKF<3, 1, 1>::inverse_exception::what() const0
std::enable_if<(4)>=(0), mrs_lib::KalmanFilter<4, 1, 2>::statecov_t>::type mrs_lib::LKF<4, 1, 2>::correction_impl<4>(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::computeKalmanGain(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 4, 0, 2, 4> const&) const0
mrs_lib::LKF<4, 1, 2>::inverse_exception::what() const0
mrs_lib::LKF<4, 1, 2>::correct(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&) const0
mrs_lib::LKF<4, 1, 2>::predict(mrs_lib::KalmanFilter<4, 1, 2>::statecov_t const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, double) const0
mrs_lib::LKF<6, 2, 2>::inverse_exception::what() const0
mrs_lib::varstepLKF<2, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 2, 2, 0, 2, 2> (double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (double)> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)3
mrs_lib::LKF<2, 1, 1>::LKF()3
mrs_lib::LKF<6, 2, 2>::LKF(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 6, 2, 0, 6, 2> const&, Eigen::Matrix<double, 2, 6, 0, 2, 6> const&)60
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&)107
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5814
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&) const5814
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&) const5814
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&) const5814
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&)18374
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)18374
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) const18374
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&)102157
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) const102162
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)102175
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)113340
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&) const113407
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&) const113416
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&) const113472
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&)168228
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) const168317
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)168416
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)269572
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&) const269993
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&) const270143
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&) const270147
+
+
+ + + +
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..ee2304860c --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-01-07 21:47:14Functions:254161.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::varstepLKF<2, 1, 1>::varstepLKF(std::function<Eigen::Matrix<double, 2, 2, 0, 2, 2> (double)> const&, std::function<Eigen::Matrix<double, 2, 1, 0, 2, 1> (double)> const&, Eigen::Matrix<double, 1, 2, 1, 1, 2> const&)3
std::enable_if<(1)!=(0), Eigen::Matrix<double, 2, 1, 0, 2, 1> >::type mrs_lib::LKF<2, 1, 1>::state_predict<1>(Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&)18374
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)18374
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5814
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&)168228
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)168416
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)269572
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&)107
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&)102157
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)102175
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)113340
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&)60
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) const18374
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&) const5814
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&) const5814
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&) const5814
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&) const270147
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&) const270143
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&) const269993
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) const168317
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&) const113416
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&) const113472
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&) const113407
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) const102162
+
+
+ + + +
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..6bf411fb16 --- /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-01-07 21:47:14Functions:254161.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file LKF
+       3             :      \brief Defines LKF - a class, implementing the Linear Kalman Filter \cite LKF, as well as a few specialized variants.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : #ifndef LKFSYSTEMMODELS_H
+       7             : #define LKFSYSTEMMODELS_H
+       8             : 
+       9             : #include <mrs_lib/kalman_filter.h>
+      10             : #include <iostream>
+      11             : 
+      12             : namespace mrs_lib
+      13             : {
+      14             : 
+      15             :   /* class LKF //{ */
+      16             :   /**
+      17             :   * \brief Implementation of the Linear Kalman filter \cite LKF.
+      18             :   *
+      19             :   * The Linear Kalman filter (abbreviated LKF, \cite LKF) may be used for state filtration or estimation of linear
+      20             :   * stochastic discrete systems. It assumes that noise variables are sampled from multivariate gaussian distributions
+      21             :   * and takes into account apriori known parameters of these distributions (namely zero means and covariance matrices,
+      22             :   * which have to be specified by the user and are tunable parameters).
+      23             :   *
+      24             :   * The LKF C++ class itself is templated. This has its advantages and disadvantages. Main disadvantage is that it
+      25             :   * may be harder to use if you're not familiar with C++ templates, which, admittedly, can get somewhat messy,
+      26             :   * espetially during compilation. Another disadvantage is that if used unwisely, the compilation times can get
+      27             :   * much higher when using templates. The main advantage is compile-time checking (if it compiles, then it has
+      28             :   * a lower chance of crashing at runtime) and enabling more effective optimalizations during compilation. Also in case
+      29             :   * of Eigen, the code is arguably more readable when you use aliases to the specific Matrix instances instead of
+      30             :   * having Eigen::MatrixXd and Eigen::VectorXd everywhere.
+      31             :   *
+      32             :   * \tparam n_states         number of states of the system (length of the \f$ \mathbf{x} \f$ vector).
+      33             :   * \tparam n_inputs         number of inputs of the system (length of the \f$ \mathbf{u} \f$ vector).
+      34             :   * \tparam n_measurements   number of measurements of the system (length of the \f$ \mathbf{z} \f$ vector).
+      35             :   *
+      36             :   */
+      37             :   template <int n_states, int n_inputs, int n_measurements>
+      38             :   class LKF : public KalmanFilter<n_states, n_inputs, n_measurements>
+      39             :   {
+      40             :   public:
+      41             :     /* LKF definitions (typedefs, constants etc) //{ */
+      42             :     static constexpr int n = n_states;            /*!< \brief Length of the state vector of the system. */
+      43             :     static constexpr int m = n_inputs;            /*!< \brief Length of the input vector of the system. */
+      44             :     static constexpr int p = n_measurements;      /*!< \brief Length of the measurement vector of the system. */
+      45             :     using Base_class = KalmanFilter<n, m, p>; /*!< \brief Base class of this class. */
+      46             : 
+      47             :     using x_t = typename Base_class::x_t;                /*!< \brief State vector type \f$n \times 1\f$ */
+      48             :     using u_t = typename Base_class::u_t;                /*!< \brief Input vector type \f$m \times 1\f$ */
+      49             :     using z_t = typename Base_class::z_t;                /*!< \brief Measurement vector type \f$p \times 1\f$ */
+      50             :     using P_t = typename Base_class::P_t;                /*!< \brief State uncertainty covariance matrix type \f$n \times n\f$ */
+      51             :     using R_t = typename Base_class::R_t;                /*!< \brief Measurement noise covariance matrix type \f$p \times p\f$ */
+      52             :     using Q_t = typename Base_class::Q_t;                /*!< \brief Process noise covariance matrix type \f$n \times n\f$ */
+      53             :     using statecov_t = typename Base_class::statecov_t;  /*!< \brief Helper struct for passing around the state and its covariance in one variable */
+      54             : 
+      55             :     typedef Eigen::Matrix<double, n, n> A_t;  /*!< \brief System transition matrix type \f$n \times n\f$ */
+      56             :     typedef Eigen::Matrix<double, n, m> B_t;  /*!< \brief Input to state mapping matrix type \f$n \times m\f$ */
+      57             :     typedef Eigen::Matrix<double, p, n> H_t;  /*!< \brief State to measurement mapping matrix type \f$p \times n\f$ */
+      58             :     typedef Eigen::Matrix<double, n, p> K_t;  /*!< \brief Kalman gain matrix type \f$n \times p\f$ */
+      59             : 
+      60             :   /*!
+      61             :     * \brief This exception is thrown when taking the inverse of a matrix fails.
+      62             :     *
+      63             :     * You should catch this exception in your code and act accordingly if it appears
+      64             :     * (e.g. reset the state and covariance or modify the measurement/process noise parameters).
+      65             :     */
+      66             :     struct inverse_exception : public std::exception
+      67             :     {
+      68             :     /*!
+      69             :       * \brief Returns the error message, describing what caused the exception.
+      70             :       *
+      71             :       * \return  The error message, describing what caused the exception.
+      72             :       */
+      73           0 :       const char* what() const throw()
+      74             :       {
+      75           0 :         return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
+      76             :       }
+      77             :     };
+      78             :     //}
+      79             : 
+      80             :   public:
+      81             :   /*!
+      82             :     * \brief Convenience default constructor.
+      83             :     *
+      84             :     * This constructor should not be used if applicable. If used, the main constructor has to be called afterwards,
+      85             :     * before using this class, otherwise the LKF object is invalid (not initialized).
+      86             :     */
+      87           3 :     LKF(){};
+      88             : 
+      89             :   /*!
+      90             :     * \brief The main constructor.
+      91             :     *
+      92             :     * \param A             The state transition matrix.
+      93             :     * \param B             The input to state mapping matrix.
+      94             :     * \param H             The state to measurement mapping matrix.
+      95             :     */
+      96         167 :     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      389214 :     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      389214 :       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      270479 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140      270479 :       statecov_t ret;
+     141      270114 :       ret.x = state_predict(A, sc.x, B, u);
+     142      270217 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143      269997 :       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      288965 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156      288965 :       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      288759 :     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      288759 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177      388726 :     static R_t invert_W(R_t W)
+     178             :     {
+     179      388726 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180      389080 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183       20239 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184       20255 :         W += 1e-9 * ident;
+     185       20260 :         qr.compute(W);
+     186       20265 :         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      387145 :       const R_t W_inv = qr.inverse();
+     193      778258 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198      389429 :     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      389429 :       const R_t W = H * sc.P * H.transpose() + R;
+     202      388932 :       const R_t W_inv = invert_W(W);
+     203      389186 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204      778054 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210      389377 :     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      389377 :       statecov_t ret;
+     214      389117 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215      388555 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216      388825 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217      777336 :       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       18374 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       18374 :       statecov_t ret;
+     319       18374 :       A_t A = m_generateA(dt);
+     320       18374 :       B_t B = m_generateB(dt);
+     321       18374 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       18374 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       36748 :       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..1c4df6e724 --- /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-01-07 21:47:14Functions: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&)107
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)107
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)107
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)107
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)107
mrs_lib::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)888
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)136602
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)136605
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)136605
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)136605
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)136605
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)136606
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)136606
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)136606
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)136819
+
+
+ + + +
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..12260a557c --- /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-01-07 21:47:14Functions: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&)136605
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)136605
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&)136605
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)107
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)107
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)136602
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)136606
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)136606
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&)136606
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)107
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)107
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)107
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)136605
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&)136819
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&)888
+
+
+ + + +
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..e63e128a06 --- /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-01-07 21:47:14Functions: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      136819 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
+      30             : 
+      31      136819 :   double x = data.x;
+      32      136819 :   double y = data.y;
+      33      136819 :   double z = data.z;
+      34             : 
+      35      273637 :   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      136606 : std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
+      96             : 
+      97      136606 :   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      136605 : double getHeading(const geometry_msgs::Pose& data) {
+     124             : 
+     125      136605 :   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      136606 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
+     184             : 
+     185      136606 :   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      136605 : double getHeading(const geometry_msgs::PoseWithCovariance& data) {
+     212             : 
+     213      136605 :   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      136605 : std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
+     336             : 
+     337      136605 :   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      136606 : std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
+     348             : 
+     349      136606 :   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      136602 : double getHeading(const nav_msgs::Odometry& data) {
+     392             : 
+     393      136602 :   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      136605 : double getHeading(const nav_msgs::OdometryConstPtr& data) {
+     404             : 
+     405      136605 :   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         888 : geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
+     448             : 
+     449         888 :   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         107 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
+     480             : 
+     481         107 :   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         107 : std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
+     579             : 
+     580         107 :   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         107 : double getHeading(const mrs_msgs::Reference& data) {
+     607             : 
+     608         107 :   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         107 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
+     639             : 
+     640         107 :   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         107 : double getHeading(const mrs_msgs::ReferenceStamped& data) {
+     667             : 
+     668         107 :   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..1356c6003c --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html @@ -0,0 +1,404 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-01-07 21:47:14Functions:678182.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
std::tuple<double, double, double, double> mrs_lib::get_mutexed<double, double, double, double>(std::mutex&, double&, double&, double&, double&)0
mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const mrs_lib::get_mutexed<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const>(std::mutex&, mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)0
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&)0
Eigen::Matrix<double, 2, 2, 0, 2, 2> mrs_lib::get_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::SpeedTrackerCommand_<std::allocator<void> >&)0
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)0
mrs_lib::KalmanFilter<2, 1, 1>::statecov_t mrs_lib::get_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 1, 0, 2, 1> >(std::mutex&, Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 2, 2, 0, 2, 2> >(std::mutex&, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 1, 0, 3, 1> >(std::mutex&, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 3, 3, 0, 3, 3> >(std::mutex&, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 3, 0, 3, 3>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 1, 0, 6, 1> >(std::mutex&, Eigen::Matrix<double, 6, 1, 0, 6, 1>, Eigen::Matrix<double, 6, 1, 0, 6, 1>&)0
auto mrs_lib::set_mutexed<Eigen::Matrix<double, 6, 6, 0, 6, 6> >(std::mutex&, Eigen::Matrix<double, 6, 6, 0, 6, 6>, Eigen::Matrix<double, 6, 6, 0, 6, 6>&)0
auto mrs_lib::set_mutexed<mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>(std::mutex&, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t, mrs_lib::KalmanFilter<2, 1, 1>::statecov_t&)0
mrs_msgs::TrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >&)3
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)18
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)57
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> >&)68
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)114
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)114
mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)156
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)494
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)496
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> >&)1272
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&)2572
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&)4192
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&)7536
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)9545
auto mrs_lib::set_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)9593
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&)9677
int mrs_lib::get_mutexed<int>(std::mutex&, int&)10390
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)11531
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&)14027
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> >&)18464
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)18635
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&)21370
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)39936
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&)40099
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&)40674
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)40674
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> >&)44585
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)47759
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)47759
auto mrs_lib::set_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >, mrs_msgs::TrackerCommand_<std::allocator<void> >&)62198
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)62344
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)73384
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> >&)79755
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)82253
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>&)102133
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&)112195
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&)113275
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&)117740
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>&)127800
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>&)135365
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> >&)137345
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>&)150304
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)160393
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&)164018
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>&)167914
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&)173023
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&)188962
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&)207784
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&)215527
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&)215615
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> >&)226540
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> >&)306199
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&)342142
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> > >&)368897
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)410355
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)412375
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&)437866
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&)438616
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> >&)539964
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)544249
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)559399
double mrs_lib::get_mutexed<double>(std::mutex&, double&)568729
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)579286
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)653165
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&)1402438
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&)1435819
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&)1542536
+
+
+ + + +
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..eced92c9e3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func.html @@ -0,0 +1,404 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-01-07 21:47:14Functions:678182.7 %
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&)40674
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>&)135365
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&)173023
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>&)127800
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&)4192
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)47759
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)11531
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)18635
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&)14027
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&)1435819
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&)112195
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&)342142
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&)117740
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&)164018
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&)113275
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&)207784
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&)1402438
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&)1542536
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&)9677
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&)188962
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)412375
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)410355
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)47759
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&)40099
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&)21370
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)73384
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)39936
mrs_uav_trajectory_generation::drsConfig mrs_lib::get_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig&)18
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)559399
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>&)167914
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>&)102133
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>&)150304
mrs_msgs::TrackerCommand_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >&)3
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)496
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)494
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)160393
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> >&)40674
mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)156
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)0
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)82253
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)544249
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&)438616
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&)215615
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> >&)44585
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> >&)137345
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> > >&)368897
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)9545
double mrs_lib::get_mutexed<double>(std::mutex&, double&)568729
int mrs_lib::get_mutexed<int>(std::mutex&, int&)10390
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)653165
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)62344
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&)7536
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&)2572
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)114
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)114
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)57
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> >&)79755
auto mrs_lib::set_mutexed<mrs_msgs::TrackerCommand_<std::allocator<void> > >(std::mutex&, mrs_msgs::TrackerCommand_<std::allocator<void> >, mrs_msgs::TrackerCommand_<std::allocator<void> >&)62198
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> >&)1272
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> >&)306199
auto mrs_lib::set_mutexed<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >(std::mutex&, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >, mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> >&)9593
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> >&)68
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> >&)539964
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> >&)226540
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&)437866
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&)215527
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> >&)18464
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)579286
+
+
+ + + +
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..42fad11674 --- /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-01-07 21:47:14Functions:678182.7 %
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      573006 : std::tuple<Args...> get_mutexed(std::mutex& mut, Args&... args) {
+      49             : 
+      50     1146015 :   std::scoped_lock lock(mut);
+      51             : 
+      52      573009 :   std::tuple result = std::tuple(args...);
+      53             : 
+      54     1146018 :   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    10940789 : T get_mutexed(std::mutex& mut, T& arg) {
+      69             : 
+      70    17921760 :   std::scoped_lock lock(mut);
+      71             : 
+      72    21874859 :   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     2557363 : auto set_mutexed(std::mutex& mut, const T what, T& where) {
+     117             : 
+     118     4513711 :   std::scoped_lock lock(mut);
+     119             : 
+     120     2552182 :   where = what;
+     121             : 
+     122     5103939 :   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..879e06031a --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html @@ -0,0 +1,368 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24828188.3 %
Date:2024-01-07 21:47:14Functions:707297.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)0
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
void mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)1
void mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)1
void mrs_lib::ParamLoader::loadMatrixStatic<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)1
void mrs_lib::ParamLoader::loadMatrixStatic<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
void mrs_lib::ParamLoader::loadMatrixDynamic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)1
void mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
mrs_lib::ParamLoader::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)5
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
std::pair<XmlRpc::XmlRpcValue, bool> mrs_lib::ParamLoader::load<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)8
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)16
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)67
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)77
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)168
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)170
void mrs_lib::ParamLoader::loadMatrixStatic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)228
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)228
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)228
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&)285
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&)340
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixX<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)400
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&)627
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)627
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> >&)627
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)968
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1175
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1465
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&)2008
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)2009
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> > > >&)2009
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)2234
mrs_lib::ParamLoader::loadedSuccessfully()2574
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)2903
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)3188
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)3188
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)3375
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)6232
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6239
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> >&)9176
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&)9478
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)9478
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)10006
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)10006
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)22519
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)23374
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)23374
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)49101
+
+
+ + + +
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..9c9376df07 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func.html @@ -0,0 +1,368 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24828188.3 %
Date:2024-01-07 21:47:14Functions:707297.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > mrs_lib::ParamLoader::loadParam2<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)285
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)77
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&)2008
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&)9478
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)10006
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&)340
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&)627
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)23374
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)0
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)3188
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6239
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixX<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)400
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)8
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
void mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)1
void mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)1
void mrs_lib::ParamLoader::loadMatrixStatic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)228
void mrs_lib::ParamLoader::loadMatrixStatic<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)1
void mrs_lib::ParamLoader::loadMatrixStatic<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
void mrs_lib::ParamLoader::loadMatrixDynamic<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, int, int)1
void mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)228
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)2
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)168
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
mrs_lib::ParamLoader::loadedSuccessfully()2574
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1465
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&)49101
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)228
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixStatic_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)170
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)9478
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)2009
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)627
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)10006
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)23374
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)3188
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)67
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> >&)9176
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&)16
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> > > >&)2009
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> >&)627
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)6232
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)3375
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)22519
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)2903
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1175
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)968
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)2234
+
+
+ + + +
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..8332b09413 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html @@ -0,0 +1,1397 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:24828188.3 %
Date:2024-01-07 21:47:14Functions:707297.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.gcov.png b/mrs_lib/include/mrs_lib/param_loader.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..22a8302368250acc0e6662b3868567977bab6d44 GIT binary patch literal 4285 zcmV;u5JK;XP)wjsdp^?$tW|NWN;r}Phq>izIR*RKAe-VH~sCA2gh$2??+gu}rDp{3~o zH4%>57990Wclj2Cis6qg0NCE`^rh&4x0suiJN|JBG!kE^TT(e93-B`# zYi6GWE?@!BD9{#1s2_}4OUS_t#iQk@zP5AKrc{}5ODCr!Qx&0#(jak_Q!oxjH=0RF zX&a2Dqn5$wCMHG|LF3ugO=~aI-Dwn4ffb{$%oo4SN@djbxaZ(5uWhshbY%^RGPh-D#ZVn9G>T?!irV=7n@zu=NHM*t_r|q z<+#B61%@?S2BRc=T;O+lx1^>G0v8P1pR&$OVLzHplU&Dy%V-`~!UbirbeR4l)XB#( zm|HZPNtErbMo=V{bC+dmTG36*VC>2zes5bjQn^x}&Q&gm1)326hieq8P{J2P1Z)e~ zkzw~!GOZl4^r~n$(P6Qm{fS5zG#L_=C}N=7CbVT|SmRhrpS~5pQg%L#y2>m^P3yX; zPsJ@$IOTPXdSCQ$uZWnTP(ei@FDkO<XMX`V~E$Y_1jtr#+(rEpN4VqrE0 zbE8qLzXyFJ>(-`xJnkv2_0e(9yqsW~ZxRnYKR7LPodXIS;R=R1HCZ3b-5$=22~~4# ziBbf9T8!h&Xn3Y6ye}#2nwN!Sy3`1DQA%V<>M$h4WOSXJnFhCza5o$?1f|K{)5q?d z2tutKw=>g#nQdm~e)uS+a2yQl+O|(&IWtndS26HhtD1h|6e?_~bqdG1&k2K3hXB$@ z^_$k3X7$wy6~sMb)6$$QTVs(Cn%!_$8T4+rRceCwKUmQ^B||zoF3cUf(D_2sjgGxA zq;*0y9uFUNmH}YN*~{G1r}Smx6By_F#iJ08z4DRuapsVT)M$}}JvT#<)KDMRKB|<) z8LSyJJs7-RJchLJRG8s{zC;+@z~l^<`$IELqY0b~TU+R_ISyYx$+26;cL9zJTylr_ zX306C1yID{sy-%*`>j|m&=&VknrlepaXUfa2@g484Gp}T0*DP6yPPwbmr-cGUi;iI z+lvU07JA5AH#tJ)wmRaJ^O2>R`Ev-fZ9H1A7BCwQMmg#@HGx!7{_}Enu}(3F>rF;# zZ97Q(@a7-bfRa!lgAWWrl5YIKI$l(hZ%O#`(^-uZ$~DlLFQs<5ER$N2t!RixMvF(t z@ur-bl~BcU6^}~d8DUT2NH~=EN~n^!JEL5vyz;DhV<4o!MY7PT(V%kugG@3G(LQiM zb5E6yb#;%!nYkA*7^+;zaNxXBA7|)U#{$u)=iF0^nnyWmSQcB*MBW=5?=nAHpmMNm zIP%|Hdd6~`vx!`*N%JCPZ<6CITkR0q>?1l(Tu84juZS=nwgXlpDU>+KuG7aT@_7VZ zm7zfZ~)GMqJftRD=(2iz|1UiGx86jMbu_CL|u zcWwArRxe0Jg70#R3=E04G>M(VxK(#uR`U#Tu^rXZj@&?zW$NR1tWA7@m-?>7&knoz z^MgO~&f+Z?H8A_h2cY9v_{Gw@HU(+%g)Z&N_9P?_Qw2Rjulop}yLN0J%oO3??Y7PB z7qb6g*^_d~wV`%$Z|UO+OOSw5SRrna8ESjBurS%8rOkIE@TEbyExI&#o>n3m&9u$C zioKU;P#QC%Nx805+arrD9%=wfY0a+%AU2*3OC)FH*TeNp;z&Ij5ze)?svMUTg%;i& zmgDbcLfz$VrNrBgeWZUxsBe0KzK=pKu_%ejMNMZ#X`KP zwr8yj4?1e;iXAOXqlM>1wDXSxmoF6-LGV{;jqaI`Sl19C^93@7Uww_hTUjbR1?4v9L3e|+wqX*_3?Hap&S$b)&Om9YgU`@(>)@LrdK6CuCi<7njO|Cd<2#fLA`b>|vZGC1= zvw;>sOiMx7*JvK~8!qH*cA}I`KQH^3CM`>S(*a|Mk(0G?Ow=C1F3#x!$UAv(8O0Y9 zl3{2Om-mSIw5x8m)Qprx&!|Xx3aBKjrGZB6&aOEu)_`>() zpURne2~hkW=Q=xofmH%yg-H}&f}rdT%y{-gepYMs!%u&b1heKtI2vIl5<@=UGM@x1 za071A#Jw3=8!I=xrO3A&Hc~HZS#ch&EWE@+eiuMbv0bsIyC#m}$mQ$^R*0~mlS+ot z#P?g)cF_srdk{x;%crdZ9ETr!z)r=y5GL|SQov?B(k5Rh%>j;bLiB#3L|7a!>-HF_ ztU~aJNx93)P%{v}%o-8~2ah#kwmlx*6-N~aC17O_xxyqWL|DndDz78-xMH(5A7mWT zCd57H5i#+m@MT^(8&m*95l~{c_5ogO8Z0Hf8gK`&-4U=eBt*c2X2qF(A+JT$D!xk$Ows}=^Y-24!3&~AJBL) zXHR(~ErQwl7`wN=lHHQ7~s?g-x-hGeNR=mtPQ8Sc?odDxIGy*A>1GOZ83k40<>~~o$jNhFOg8s z(W)~vyK?$*51zY5+@o|i4Sj=9*>cm^duk>S z;nR%$tK#vB0sFHpZrAD_YH=gmkHvi#gkLJWt`I6)R#;qdQsnD|*JMc;bNe_NcQtXl zdAcJB^&A8CK7!19RE=qZXDIxRomX#Dbm4hMJ(%*Iqybp>Q>X}&JuIp@@+}Z{znZ&j z`bgzcUmt}3d6d!vt{jVY&<_K5+tOPBSIx2RKBoO1X%Kq5Tg8D+64T7bNg{0 zxfY7fSl!2jyoPP-zPCAqOcA~4WVi~U&hh|Dosqh}NqJ9oHEH2diOIM`KCCO{w zjeUFtw5Y3HY6lFk+o(uQhfu(ggsv!K;YV@ZAJ>4 z3QcpBZ`;WZ@RQl!YznXQ?$N0h7f^<fT&Ek+F zNv7(AtAusWesoQ$A869K%Qh{{0!?K1N%vmN7d|WJl1=yYra`#Xn;klXFRKC@>rA@& zpw6%l{3K^@=su403h>!+&Sh7H?)N)az6DCeZuH}bRH}>=KdIFFxR0lJ1@XDVmjOra zqpdSyYt=D2BmKxV5+8tQ;5Q!Kh>_zVo{PPgyN0@6-g7;a_&|@zK5hx6NP(sM%g58< z1$^)KBSBmdy%FJ6gSgA7!uxnGmpC@{rfj;YH*LZ~Z`|35PgQ5)%?EWx#Ci)6estzt z+{ck#0fM+IBtEhZfIl8ieE5tLQnM>2J{MJm_i-ObdWFE|{n?4n_Uoc;2=D2P^dqmd za~~y$wj6Oxpgc@n>8CRxoFF)PiEYpuPaG$VaS+^zHqZc9>O|=AZTvjIXyxz8+&k85 zQcf?_Ab?qjgfz~OPP+il0)O6!2m4OeU?UBaz3ZNI zdlB_>ltb=A+w?iv@HYI;wo@9>vyJR)E94i>hTj^GI{VlFE(yxq#}f?c0JujSwo&^+ zl;^I+9nDd_a(Db#{`Y4qV%n7zhA?q(XVrT&1DeP=Rjw@B(_a7MxxVrg8W!#Ym2{3W f1#9!|>E-J`6YuqhU_-h-00000NkvXXu0mjf4lXN) literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html new file mode 100644 index 0000000000..1c0430ed49 --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html @@ -0,0 +1,548 @@ + + + + + + + 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-01-07 21:47:14Functions:11611799.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()0
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<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()13
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()13
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()26
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()58
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()60
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()98
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()115
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()118
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()118
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()118
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()167
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()171
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()173
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()178
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()227
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()227
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()228
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()228
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()236
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()286
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()332
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()335
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()335
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()342
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()342
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()342
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()351
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()396
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()454
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()454
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()454
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()456
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()563
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()616
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()684
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()908
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()948
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()96041
+
+
+ + + +
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..ab070b0ece --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,548 @@ + + + + + + + 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-01-07 21:47:14Functions:11611799.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()228
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()456
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()118
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()178
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()342
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()684
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()616
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()948
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()227
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()454
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::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()454
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()908
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()396
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()563
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()115
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()173
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()335
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()335
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()171
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()98
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()118
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()236
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()351
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()96041
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()13
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()26
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()57
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()114
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()171
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()342
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()228
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()60
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()342
mrs_lib::PublisherHandler_impl<mrs_msgs::BumperStatus_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()332
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()227
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()454
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()167
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()58
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()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()57
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()118
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()286
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()13
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()57
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()57
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()171
+
+
+ + + +
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..4c8e721c94 --- /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-01-07 21:47:14Functions:11611799.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addMeasurement<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::Repredictor(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)1
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::info_t::info_t(ros::Time const&)2
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)2
std::enable_if<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&)92
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&)225
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&)282
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&)282
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&)564
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&)564
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)634
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&)2053
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&)7537
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&)17306
+
+
+ + + +
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..8c34907c72 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func.html @@ -0,0 +1,304 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-01-07 21:47:14Functions:205635.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&)5150
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)17306
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&)7537
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)634
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&)2053
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&)282
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&)564
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&)92
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&)282
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&)225
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&)564
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&, unsigned int)2
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<2, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<2, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<2, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<2, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::correctFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::predictFrom(mrs_lib::KalmanFilter<3, 1, 1>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addMeasurement<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<3, 1, 1>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<3, 1, 1>, false>::Repredictor(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<3, 1, 1> > const&, unsigned int)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::correctFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::predictFrom(mrs_lib::KalmanFilter<6, 2, 2>::statecov_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, ros::Time const&, ros::Time const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addMeasurement<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&, double const&)0
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::updateUsing(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, int const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::addInfo(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, boost::cb_details::iterator<boost::circular_buffer<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t, std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> >, boost::cb_details::nonconst_traits<boost::cb_details::allocator_traits<std::allocator<mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t> > > > const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::earlier(mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&, mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::info_t const&)0
std::enable_if<!(false), mrs_lib::KalmanFilter<6, 2, 2>::statecov_t>::type mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::predictTo<false>(ros::Time const&)0
mrs_lib::Repredictor<mrs_lib::LKF<6, 2, 2>, false>::Repredictor(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 6, 6, 0, 6, 6> const&, ros::Time const&, std::shared_ptr<mrs_lib::LKF<6, 2, 2> > const&, unsigned int)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html new file mode 100644 index 0000000000..d18e49adfb --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_lib/include/mrs_lib/repredictor.h.gcov.html b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html new file mode 100644 index 0000000000..c1aff09c8b --- /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-01-07 21:47:14Functions:205635.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #ifndef REPREDICTOR_H
+       2             : #define REPREDICTOR_H
+       3             : 
+       4             : #include <Eigen/Dense>
+       5             : #include <boost/circular_buffer.hpp>
+       6             : #include <std_msgs/Time.h>
+       7             : #include <functional>
+       8             : #include <ros/ros.h>
+       9             : #include <mrs_lib/utils.h>
+      10             : 
+      11             : namespace mrs_lib
+      12             : {
+      13             :   /**
+      14             :    * \brief Implementation of the Repredictor for fusing measurements with variable delays.
+      15             :    *
+      16             :    * A standard state-space system model is assumed for the repredictor with system inputs and measurements,
+      17             :    * generated from the system state vector. The inputs and measurements may be delayed with varying durations (an older measurement may become available after
+      18             :    * a newer one). A typical use-case scenario is when you have one "fast" but imprecise sensor and one "slow" but precise sensor and you want to use them both
+      19             :    * to get a good prediction (eg. height from the Garmin LiDAR, which comes at 100Hz, and position from a SLAM, which comes at 10Hz with a long delay). If the
+      20             :    * slow sensor is significantly slower than the fast one, fusing its measurement at the time it arrives without taking into account the sensor's delay may
+      21             :    * significantly bias your latest estimate.
+      22             :    *
+      23             :    * To accomodate this, the Repredictor keeps a buffer of N last inputs and measurements (N is specified
+      24             :    * in the constructor). This buffer is then used to re-predict the desired state to a specific time, as
+      25             :    * requested by the user. Note that the re-prediction is evaluated in a lazy manner only when the user requests it,
+      26             :    * so it goes through the whole history buffer every time a prediction is requested.
+      27             :    *
+      28             :    * The Repredictor utilizes a fusion Model (specified as the template parameter), which should implement
+      29             :    * the predict() and correct() methods. This Model is used for fusing the system inputs and measurements
+      30             :    * as well as for predictions. Typically, this Model will be some kind of a Kalman Filter (LKF, UKF etc.).
+      31             :    * \note The Model should be able to accomodate predictions with varying time steps in order for
+      32             :    * the Repredictor to work correctly (see eg. the varstepLKF class).
+      33             :    *
+      34             :    * \tparam Model                  the prediction and correction model (eg. a Kalman Filter).
+      35             :    * \tparam disable_reprediction   if true, reprediction is disabled and the class will act like a dumb LKF (for evaluation purposes).
+      36             :    *
+      37             :    */
+      38             :   template <class Model, bool disable_reprediction=false>
+      39             :   class Repredictor
+      40             :   {
+      41             :   public:
+      42             :     /* states, inputs etc. definitions (typedefs, constants etc) //{ */
+      43             : 
+      44             :     using x_t = typename Model::x_t;                  /*!< \brief State vector type \f$n \times 1\f$ */
+      45             :     using u_t = typename Model::u_t;                  /*!< \brief Input vector type \f$m \times 1\f$ */
+      46             :     using z_t = typename Model::z_t;                  /*!< \brief Measurement vector type \f$p \times 1\f$ */
+      47             :     using P_t = typename Model::P_t;                  /*!< \brief State uncertainty covariance matrix type \f$n \times n\f$ */
+      48             :     using R_t = typename Model::R_t;                  /*!< \brief Measurement noise covariance matrix type \f$p \times p\f$ */
+      49             :     using Q_t = typename Model::Q_t;                  /*!< \brief Process noise covariance matrix type \f$n \times n\f$ */
+      50             :     using statecov_t = typename Model::statecov_t;    /*!< \brief Helper struct for passing around the state and its covariance in one variable */
+      51             :     using ModelPtr = typename std::shared_ptr<Model>; /*!< \brief Shorthand type for a shared pointer-to-Model */
+      52             : 
+      53             :     //}
+      54             : 
+      55             :     /* predictTo() method //{ */
+      56             :     /*!
+      57             :      * \brief Estimates the system state and covariance matrix at the specified time.
+      58             :      *
+      59             :      * The measurement and system input histories are used to estimate the state vector and
+      60             :      * covariance matrix values at the specified time, which are returned.
+      61             :      *
+      62             :      * \param to_stamp   The desired time at which the state vector and covariance matrix should be estimated.
+      63             :      * \return           Returns the estimated state vector and covariance matrix in a single struct.
+      64             :      *
+      65             :      */
+      66             :     template<bool check=disable_reprediction>
+      67         102 :     std::enable_if_t<!check, statecov_t> predictTo(const ros::Time& to_stamp)
+      68             :     {
+      69         102 :       assert(!m_history.empty());
+      70         102 :       auto hist_it = std::begin(m_history);
+      71         102 :       if (hist_it->is_measurement)
+      72             :       {
+      73             :         // if the first history point is measurement, don't use it for correction (to prevent it from being used twice)
+      74           0 :         hist_it->is_measurement = false;
+      75             :       }
+      76             :       // cur_stamp corresponds to the time point of cur_sc estimation
+      77         102 :       auto cur_stamp = hist_it->stamp;
+      78             :       // cur_sc is the current state and covariance estimate
+      79         102 :       auto cur_sc = m_sc;
+      80       17204 :       do
+      81             :       {
+      82       17306 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       17306 :         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       17306 :         ros::Time next_stamp = to_stamp;
+      90       17306 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       17204 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       17306 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       17306 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       17306 :         hist_it++;
+      99       17306 :       } 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         564 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         564 :       assert(!m_history.empty());
+     120         564 :       const auto& info = m_history.front();
+     121         564 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         564 :       sc.stamp = to_stamp;
+     123         564 :       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        7637 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        7437 :           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         225 :     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         225 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         225 :       m_history.front().u = u;
+     172         225 :       m_history.front().Q = Q;
+     173         225 :       m_history.front().stamp = stamp;
+     174         225 :       m_history.front().predict_model = model;
+     175         225 :     }
+     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          92 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222          92 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224          92 :       m_history.front().u = u;
+     225          92 :       m_history.front().stamp = stamp;
+     226          92 :       m_history.front().predict_model = model;
+     227          92 :     }
+     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         282 :     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         282 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         282 :       auto& info = m_history.front();
+     327         282 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         282 :       const auto sc = predictTo(to_stamp);
+     329         282 :       info.z = z;
+     330         282 :       info.R = R;
+     331         282 :       info.stamp = to_stamp;
+     332         282 :       info.is_measurement = true;
+     333         282 :       info.meas_id = meas_id;
+     334         282 :       info.correct_model = model;
+     335         282 :       m_sc = correctFrom(sc, info);
+     336         282 :     }
+     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         636 :       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        7537 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        7537 :         u = info.u;
+     433        7537 :         Q = info.Q;
+     434        7537 :         predict_model = info.predict_model;
+     435        7537 :       };
+     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        2053 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2053 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       17870 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       35740 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       17870 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       35740 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5432 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5432 :       assert(meas.is_measurement);
+     544       10864 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5432 :       auto sc_tmp = sc;
+     546       10864 :       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..324e16ba36 --- /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-01-07 21:47:14Functions: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..ab1e5549c9 --- /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-01-07 21:47:14Functions: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..3ef306830d --- /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-01-07 21:47:14Functions: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..bff887314d --- /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-01-07 21:47:14Functions: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..a62a1b3128 --- /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-01-07 21:47:14Functions: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..a55032beca --- /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-01-07 21:47:14Functions: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..0da47d9283 --- /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-01-07 21:47:14Functions: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..8dcbaafe85 --- /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-01-07 21:47:14Functions: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..7403d16266 --- /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-01-07 21:47:14Functions: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..becd2e98b2 --- /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-01-07 21:47:14Functions: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..c05ccf6c64 --- /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-01-07 21:47:14Functions: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..d7b0ec274f --- /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-01-07 21:47:14Functions: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..4298b791ee --- /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-01-07 21:47:14Functions: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&)3089225
+
+
+ + + +
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..64269d2e9e --- /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-01-07 21:47:14Functions: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&)3089225
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..2c3607e8c2 --- /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-01-07 21:47:14Functions: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     3089225 :     time_point(const std::string& label) : ros_time(ros::Time::now()), chrono_time(std::chrono::steady_clock::now()), label(label) {
+      93     3089265 :     }
+      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-01-07 21:47:14Functions:2020100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()57
mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv>::~ServiceClientHandler()98
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()114
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()114
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()114
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()116
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()116
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::~ServiceClientHandler()196
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()344
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()344
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()428
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()671
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()671
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()786
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1613
+
+
+ + + +
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..7684457abc --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html @@ -0,0 +1,160 @@ + + + + + + + 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-01-07 21:47:14Functions:2020100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()57
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()114
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()57
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()114
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()57
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()114
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::~ServiceClientHandler()196
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()116
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()428
mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv>::~ServiceClientHandler()98
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()344
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()786
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()671
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1613
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()57
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()116
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()344
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()671
+
+
+ + + +
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..0be9c3615d --- /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-01-07 21:47:14Functions:2020100.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        1302 :   ~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        1302 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139        3463 :   ~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..097b7025dd --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,2916 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:394979.6 %
Date:2024-01-07 21:47:14Functions:35670950.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcTfToWorld<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::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::ObstacleSectors_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::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> > >::start()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::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> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getMsg()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<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const1
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::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()2
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<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<std_msgs::Bool_<std::allocator<void> > >::start()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >&&)2
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<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::TrackerCommand_<std::allocator<void> > >::hasMsg() const3
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<std_msgs::Bool_<std::allocator<void> > >::~SubscribeHandler()4
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const4
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::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::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*)13
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*)13
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()13
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)13
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*)13
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*)13
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()() const13
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()() const13
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()() const13
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&)15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const41
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()52
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*)54
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*)54
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const54
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const54
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()() const54
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>*)56
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>*)56
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>*)56
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>*)56
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()() const56
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()() const56
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*)57
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*)57
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*)57
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*)57
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&)57
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*)57
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*)57
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&)57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)57
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*)57
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*)57
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&)57
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*)57
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*)57
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*)57
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*)57
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*)57
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*)57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)57
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*)57
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*)57
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*)57
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*)57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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()() const57
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&)58
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const58
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const58
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()() const58
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&)60
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>*)60
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*)60
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>*)60
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*)60
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*)60
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*)60
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*)60
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*)60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)60
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()() const60
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()() const60
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()() const60
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()() const60
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()() const60
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()() const60
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()() const60
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()61
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*)61
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*)61
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()() const61
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()65
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const65
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()67
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()73
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const74
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()98
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()101
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&)101
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>*)101
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>*)101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)101
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()() const101
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const108
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()109
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()114
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&)114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)114
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)114
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()114
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&)114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()114
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()114
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)114
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()() const114
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()117
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&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()118
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()120
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()124
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&)127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)127
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()128
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()166
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()170
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&)171
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()171
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&)171
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&)171
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()() const171
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()() const171
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()() const171
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()177
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&)177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)177
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()179
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const181
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()184
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&)184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)184
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()186
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)186
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()() const186
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()212
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()212
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()212
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()224
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&)224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()224
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()224
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)224
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()() const224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()227
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()227
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()227
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()227
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()227
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()227
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()228
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()228
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&)228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)228
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()228
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()235
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()238
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()243
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)243
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()() const243
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const270
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()284
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()285
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()285
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()285
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()293
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()295
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()326
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()332
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()347
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()348
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&)348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)348
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()352
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()361
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()448
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()451
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()461
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()470
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()531
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const538
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()584
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()638
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()652
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()803
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()1151
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()1344
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()1437
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()2870
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const4317
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const12401
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const29534
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()29599
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()30275
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()34362
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const36633
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()99536
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const99537
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()112356
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const127920
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const174354
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const207632
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const232070
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()243021
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()266701
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const317900
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()430763
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const551462
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const14594272
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const14594272
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const14600658
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const14633161
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const14771296
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const63908647
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()78121700
+
+
+ + + +
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..1ae5e12fad --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,2916 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:394979.6 %
Date:2024-01-07 21:47:14Functions:35670950.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()224
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&)224
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()227
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*)57
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*)57
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*)54
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>*)56
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*)57
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*)57
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*)54
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>*)56
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()451
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)224
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()114
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&)114
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()114
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&)57
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*)57
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*)57
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()228
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)114
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()101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()2870
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&)101
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()227
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>*)101
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>*)101
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()328
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)101
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()227
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()227
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()61
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()1344
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&)57
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()228
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>*)56
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>*)56
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()285
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)57
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()184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()112356
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&)184
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()347
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&)60
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>*)60
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*)60
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>*)60
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*)60
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()531
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)184
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()348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()430763
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&)348
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()803
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&)171
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*)57
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*)60
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*)60
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*)57
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*)60
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*)60
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()1151
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)348
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()227
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()227
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()179
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()285
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&)127
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()127
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&)57
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*)57
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*)13
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*)57
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*)13
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)127
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()117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()99536
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&)117
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()118
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&)60
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()235
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)117
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()224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()243021
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()224
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&)224
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()448
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)224
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()332
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()29599
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&)228
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()228
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&)171
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()652
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)228
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()109
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()361
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()212
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)57
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()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()60
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&)60
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()120
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)60
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()57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()538
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)57
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()57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const>)> const&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)57
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)57
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()171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()170
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()114
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&)171
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()285
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)114
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()52
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getMsg()1
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()98
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()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)57
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()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)57
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()57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()3
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)57
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()295
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()34362
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()243
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&)243
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()584
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)243
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()65
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()13
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&)13
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()124
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)13
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()109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const>)> const&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()212
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)57
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()67
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()2
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&)15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()15
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
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*)13
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*)13
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()128
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)15
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()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()114
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)57
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()238
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()78121700
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()186
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&)186
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()470
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)186
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()109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const>)> const&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()57
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&)57
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()212
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)57
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()73
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()284
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()293
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()166
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()30275
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&)114
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()114
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*)57
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*)57
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*)57
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*)57
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()326
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)114
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()177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()266701
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&)177
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()461
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&)58
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*)57
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*)61
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*)57
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*)61
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()638
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)177
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::start()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<std_msgs::Bool_<std::allocator<void> > const>)> const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler()2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::~SubscribeHandler()4
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >&&)2
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()114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()21897
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()114
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&)114
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()228
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)114
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()1437
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const4317
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() const12401
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() const232070
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() const551462
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]() const58
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() const108
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const181
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() const99537
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() const207632
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const317900
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() const36633
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() const3
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() const538
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const270
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]() const58
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() const1
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() const57
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() const14633161
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() const14600658
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() const54
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const14594272
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() const41
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() const63908647
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]() const65
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const54
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const14594272
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const74
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() const127920
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const14771296
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() const174354
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::Bool_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const21897
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const29534
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()() const57
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()() const57
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()() const54
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()() const56
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()() const57
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()() const57
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()() const101
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()() const56
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()() const60
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()() const60
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()() const60
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()() const171
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()() const57
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()() const60
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()() const60
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()() const57
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()() const57
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()() const13
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()() const60
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()() const57
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()() const224
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()() const171
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()() const57
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()() const57
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()() const60
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()() const57
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()() const57
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const57
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()() const171
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()() const57
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()() const57
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()() const57
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()() const243
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const13
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()() const57
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const2
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()() const13
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()() const57
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()() const186
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()() const57
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()() const57
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()() const57
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()() const58
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()() const57
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()() const61
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<std_msgs::Bool_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const2
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()() const114
+
+
+ + + +
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..04cf877866 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:394979.6 %
Date:2024-01-07 21:47:14Functions:35670950.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines SubscribeHandler and related convenience classes for subscribing to ROS topics.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef SUBRSCRIBE_HANDLER_H
+       8             : #define SUBRSCRIBE_HANDLER_H
+       9             : 
+      10             : #include <ros/ros.h>
+      11             : #include <mrs_lib/timeout_manager.h>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             : 
+      16             :   static const ros::Duration no_timeout = ros::Duration(0);
+      17             : 
+      18             :   /* struct SubscribeHandlerOptions //{ */
+      19             :   
+      20             :   /**
+      21             :   * \brief A helper class to simplify setup of SubscribeHandler construction.
+      22             :   * This class is passed to the SubscribeHandler constructor and specifies its common options.
+      23             :   *
+      24             :   * \note Any option, passed directly to the SubscribeHandler constructor outside this structure, *OVERRIDES* values in this structure.
+      25             :   * The values in this structure can be thought of as default common values for all SubscribeHandler objects you want to create,
+      26             :   * and values passed directly to the constructor as specific options for the concrete handler.
+      27             :   *
+      28             :   */
+      29             :   struct SubscribeHandlerOptions
+      30             :   {
+      31             :     SubscribeHandlerOptions(const ros::NodeHandle& nh) : nh(nh) {}
+      32        1437 :     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    79395857 :       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   138243779 :       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      317900 :       virtual bool newMsg() const {assert(m_pimpl); return m_pimpl->newMsg();};
+     128             : 
+     129             :     /*!
+     130             :       * \brief Used to check whether getMsg() was called at least once on this SubscribeHandler.
+     131             :       *
+     132             :       * \return true if getMsg() was called at least once, otherwise false.
+     133             :       */
+     134           0 :       virtual bool usedMsg() const {assert(m_pimpl); return m_pimpl->usedMsg();};
+     135             : 
+     136             :     /*!
+     137             :       * \brief Blocks until new data becomes available or until the timeout runs out or until a spurious wake-up.
+     138             :       *
+     139             :       * \return the message if a new message is available after waking up, \p nullptr otherwise.
+     140             :       */
+     141           0 :       virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout) {assert(m_pimpl); return m_pimpl->waitForNew(timeout);};
+     142             : 
+     143             :     /*!
+     144             :       * \brief Returns time of the last received message on the topic, handled by this SubscribeHandler.
+     145             :       *
+     146             :       * \return time when the last message was received.
+     147             :       */
+     148      357670 :       virtual ros::Time lastMsgTime() const {assert(m_pimpl); return m_pimpl->lastMsgTime();};
+     149             : 
+     150             :     /*!
+     151             :       * \brief Returns the resolved (full) name of the topic, handled by this SubscribeHandler.
+     152             :       *
+     153             :       * \return name of the handled topic.
+     154             :       */
+     155         182 :       virtual std::string topicName() const {assert(m_pimpl); return m_pimpl->topicName();};
+     156             : 
+     157             :     /*!
+     158             :       * \brief Returns the subscribed (unresolved) name of the topic, handled by this SubscribeHandler.
+     159             :       *
+     160             :       * \return name of the handled topic.
+     161             :       */
+     162           0 :       virtual std::string subscribedTopicName() const {assert(m_pimpl); return m_pimpl->m_topic_name;};
+     163             : 
+     164             :     /*!
+     165             :       * \brief Enables the callbacks for the handled topic.
+     166             :       *
+     167             :       * If the SubscribeHandler object is stopped using the stop() method, no callbacks will be called
+     168             :       * until the start() method is called.
+     169             :       */
+     170        4026 :       virtual void start() {assert(m_pimpl); return m_pimpl->start();};
+     171             : 
+     172             :     /*!
+     173             :       * \brief Disables the callbacks for the handled topic.
+     174             :       *
+     175             :       * All messages after this method is called will be ignored until start() is called again.
+     176             :       * Timeout checking will also be disabled.
+     177             :       */
+     178           5 :       virtual void stop() {assert(m_pimpl); return m_pimpl->stop();};
+     179             : 
+     180             :     public:
+     181             :     /*!
+     182             :       * \brief Default constructor to avoid having to use pointers.
+     183             :       *
+     184             :       * It does nothing and the SubscribeHandler it constructs will also do nothing.
+     185             :       * Use some of the other constructors for actual construction of a usable SubscribeHandler.
+     186             :       */
+     187        5273 :       SubscribeHandler() {};
+     188             : 
+     189             :     /*!
+     190             :       * \brief Main constructor.
+     191             :       *
+     192             :       * \param options    The common options struct (see documentation of SubscribeHandlerOptions).
+     193             :       * \param topic_name Name of the topic to be handled by this subscribed.
+     194             :       * \param args       Remaining arguments to be parsed (see other constructors).
+     195             :       *
+     196             :       */
+     197             :       template<class ... Types>
+     198        3398 :       SubscribeHandler(
+     199             :             const SubscribeHandlerOptions& options,
+     200             :             const std::string& topic_name,
+     201             :             Types ... args
+     202             :           )
+     203             :       :
+     204             :         SubscribeHandler(
+     205        3398 :             [options, topic_name]()
+     206             :             {
+     207        6796 :               SubscribeHandlerOptions opts = options;
+     208        3398 :               opts.topic_name = topic_name;
+     209        6796 :               return opts;
+     210             :             }(),
+     211             :             args...
+     212        3398 :             )
+     213             :       {
+     214        3398 :       }
+     215             : 
+     216             :     /*!
+     217             :       * \brief Convenience constructor overload.
+     218             :       *
+     219             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     220             :       * \param message_callback The callback function to call when a new message is received (you can leave this argument empty and just use the newMsg()/hasMsg() and getMsg() interface).
+     221             :       *
+     222             :       */
+     223        3398 :       SubscribeHandler(
+     224             :             const SubscribeHandlerOptions& options,
+     225             :             const message_callback_t& message_callback = {}
+     226             :           )
+     227        3398 :       {
+     228        3398 :         if (options.threadsafe)
+     229             :         {
+     230        3398 :           m_pimpl = std::make_unique<ImplThreadsafe>
+     231             :             (
+     232             :               options,
+     233             :               message_callback
+     234             :             );
+     235             :         }
+     236             :         else
+     237             :         {
+     238           0 :           m_pimpl = std::make_unique<Impl>
+     239             :             (
+     240             :               options,
+     241             :               message_callback
+     242             :             );
+     243             :         }
+     244        3398 :         if (options.autostart)
+     245        3397 :           start();
+     246        3398 :       };
+     247             : 
+     248             :     /*!
+     249             :       * \brief Convenience constructor overload.
+     250             :       *
+     251             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     252             :       * \param timeout_callback The callback function to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     253             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     254             :       *
+     255             :       */
+     256             :       template <class ... Types>
+     257           1 :       SubscribeHandler(
+     258             :             const SubscribeHandlerOptions& options,
+     259             :             const timeout_callback_t& timeout_callback,
+     260             :             Types ... args
+     261             :           )
+     262             :       :
+     263             :         SubscribeHandler(
+     264           1 :             [options, timeout_callback]()
+     265             :             {
+     266           2 :               SubscribeHandlerOptions opts = options;
+     267           1 :               opts.timeout_callback = timeout_callback;
+     268           2 :               return opts;
+     269             :             }(),
+     270             :             args...
+     271           1 :             )
+     272             :       {
+     273           1 :       }
+     274             : 
+     275             :     /*!
+     276             :       * \brief Convenience constructor overload.
+     277             :       *
+     278             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     279             :       * \param timeout_callback The callback method to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     280             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     281             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     282             :       *
+     283             :       */
+     284             :       template <class ObjectType1, class ... Types>
+     285             :       SubscribeHandler(
+     286             :             const SubscribeHandlerOptions& options,
+     287             :             void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     288             :             ObjectType1* const obj1,
+     289             :             Types ... args
+     290             :           )
+     291             :       :
+     292             :         SubscribeHandler(
+     293             :             [options, timeout_callback, obj1]()
+     294             :             {
+     295             :               SubscribeHandlerOptions opts = options;
+     296             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     297             :               return opts;
+     298             :             }(),
+     299             :             args...
+     300             :             )
+     301             :       {
+     302             :       }
+     303             : 
+     304             :     /*!
+     305             :       * \brief Convenience constructor overload.
+     306             :       *
+     307             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     308             :       * \param message_callback The callback method to call when a new message is received.
+     309             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     310             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     311             :       *
+     312             :       */
+     313             :       template <class ObjectType2, class ... Types>
+     314        1463 :       SubscribeHandler(
+     315             :             const SubscribeHandlerOptions& options,
+     316             :             void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     317             :             ObjectType2* const obj2,
+     318             :             Types ... args
+     319             :           )
+     320             :       :
+     321             :         SubscribeHandler(
+     322             :             options,
+     323        2926 :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     324             :             args...
+     325        1463 :             )
+     326             :       {
+     327        1463 :       }
+     328             : 
+     329             :     /*!
+     330             :       * \brief Convenience constructor overload.
+     331             :       *
+     332             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     333             :       * \param message_callback The callback method to call when a new message is received.
+     334             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     335             :       * \param timeout_callback The callback method to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     336             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     337             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     338             :       *
+     339             :       */
+     340             :      template <class ObjectType1, class ObjectType2, class ... Types>
+     341             :      SubscribeHandler(
+     342             :            const SubscribeHandlerOptions& options,
+     343             :            void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     344             :            ObjectType2* const obj2,
+     345             :            void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     346             :            ObjectType1* const obj1,
+     347             :            Types ... args
+     348             :          )
+     349             :      :
+     350             :        SubscribeHandler(
+     351             :             [options, timeout_callback, obj1]()
+     352             :             {
+     353             :               SubscribeHandlerOptions opts = options;
+     354             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     355             :               return opts;
+     356             :             }(),
+     357             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     358             :             args...
+     359             :             )
+     360             :      {
+     361             :      }
+     362             : 
+     363             :     /*!
+     364             :       * \brief Convenience constructor overload.
+     365             :       *
+     366             :       * \param options            The common options struct (see documentation of SubscribeHandlerOptions).
+     367             :       * \param no_message_timeout If no message is received for this duration, the timeout callback function is called. If no timeout callback is specified, an error message is printed to the console.
+     368             :       * \param args               Remaining arguments to be parsed (see other constructors).
+     369             :       *
+     370             :       */
+     371             :       template<class ... Types>
+     372             :       SubscribeHandler(
+     373             :             const SubscribeHandlerOptions& options,
+     374             :             const ros::Duration& no_message_timeout,
+     375             :             Types ... args
+     376             :           )
+     377             :       :
+     378             :         SubscribeHandler(
+     379             :             [options, no_message_timeout]()
+     380             :             {
+     381             :               SubscribeHandlerOptions opts = options;
+     382             :               opts.no_message_timeout = no_message_timeout;
+     383             :               return opts;
+     384             :             }(),
+     385             :             args...
+     386             :             )
+     387             :       {
+     388             :       }
+     389             : 
+     390             :     /*!
+     391             :       * \brief Convenience constructor overload.
+     392             :       *
+     393             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     394             :       * \param timeout_manager  The manager for timeout callbacks.
+     395             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     396             :       *
+     397             :       */
+     398             :       template <class ... Types>
+     399             :       SubscribeHandler(
+     400             :             const SubscribeHandlerOptions& options,
+     401             :             mrs_lib::TimeoutManager& timeout_manager,
+     402             :             Types ... args
+     403             :           )
+     404             :       :
+     405             :         SubscribeHandler(
+     406             :             options,
+     407             :             timeout_manager = timeout_manager,
+     408             :             args...
+     409             :             )
+     410             :       {
+     411             :       }
+     412             : 
+     413        9847 :       ~SubscribeHandler() = default;
+     414             :       // delete copy constructor and assignment operator (forbid copying shandlers)
+     415             :       SubscribeHandler(const SubscribeHandler&) = delete;
+     416             :       SubscribeHandler& operator=(const SubscribeHandler&) = delete;
+     417             :       // define only move constructor and assignemnt operator
+     418           0 :       SubscribeHandler(SubscribeHandler&& other)
+     419           0 :       {
+     420           0 :         this->m_pimpl = std::move(other.m_pimpl);
+     421           0 :         other.m_pimpl = nullptr;
+     422           0 :       }
+     423        3341 :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     424             :       {
+     425        3341 :         this->m_pimpl = std::move(other.m_pimpl);
+     426        3341 :         other.m_pimpl = nullptr;
+     427        3341 :         return *this;
+     428             :       }
+     429             : 
+     430             :     private:
+     431             :       class Impl;
+     432             :       class ImplThreadsafe;
+     433             :       std::unique_ptr<Impl> m_pimpl;
+     434             :   };
+     435             :   //}
+     436             : 
+     437             : /*!
+     438             :   * \brief Helper alias for convenient extraction of handled message type from a SubscribeHandlerPtr.
+     439             :   */
+     440             :   template<typename SubscribeHandler>
+     441             :   using message_type = typename SubscribeHandler::message_type;
+     442             : 
+     443             : /*!
+     444             :   * \brief Helper function for object construstion e.g. in case of member objects.
+     445             :   * This function is useful to avoid specifying object template parameters twice - once in definition of the variable and second time during object construction.
+     446             :   * This function can deduce the template parameters from the type of the already defined object, because it returns the newly constructed object as a reference
+     447             :   * argument and not as a return type.
+     448             :   *
+     449             :   * \param object The object to be constructed.
+     450             :   * \param args   These arguments are passed to the object constructor.
+     451             :   */
+     452             :   template<typename Class, class ... Types>
+     453             :   void construct_object(
+     454             :         Class& object,
+     455             :         Types ... args
+     456             :       )
+     457             :   {
+     458             :     object = Class(args...);
+     459             :   }
+     460             : }
+     461             : 
+     462             : #include <mrs_lib/impl/subscribe_handler.hpp>
+     463             : 
+     464             : #endif // SUBRSCRIBE_HANDLER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html new file mode 100644 index 0000000000..056386c498 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html @@ -0,0 +1,136 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4fbffc4bc0b89a8b74c7423122e9f29b5abf23f7 GIT binary patch literal 1794 zcmV+d2mSboP)GAf72gj3K5r zbz2ND5}}4FQEmJ-0qHg3YyO-C@r%I16(bOAcI{2~uIrUq&|?~*Fbke#KB6~6I8Vnr z*#`C}y~&UtGp8aqYx(}~$v#>;vYCZ!J|t)Of*?k(LiRez2b~a-7TcQx@sJ(OR zSq>>wvJ`~KR`Ok-ZVp=~gM8B6WXF=d4O(*_&BL&Z@Q&A3jZ={@1qCG!No<}!R9n~W zb*{9j_GML&O^1ys1CB351|2_`Jt2(&lZCYg(hT|2F^wA_6OB9MF{bcvYuq&&R7fFo zsE;bOL1Kh32wTn^>m!tz668ytM&>Z-6K5u6q!$AOc%jcH>3fS({;hPmwMn+C2MvDs~C)tCPOw1-3bupJYJgBin5(kdU98OtK zN-*pu2VTAz1vBYHiE0uiLlN>kL#(mA6FXs73unw;3sz_yc$`3{Fzh2wZJ5Y%O{q?O z6g)UZIm3|l&g)}FbThr1oSPcWrg26f^YA!E?wTS*9S^FU0fyKGkEa3RWj`QnTtj@P)_8$ z2uGra;0+aAu({!p*B^pM3f2OmBou>R#tqPAJ6&N4mRxM#Slrw2_gpYNmF=GG1RVEwfr_%69B zg1?$lN=5+@BubI(e|^i#a1Wu_TKJigkY$BZh;SQ%asVCl1<+MtJ#PXHO}{QliD=<% z>z+eDCe1n-oV|-Sf6kQh2wK~lq+XMx=F1AC21)TJ)y+ReQUejf z5dmCx>)nM`Fd{f87~WNiSz{ifA0U9gK`C-_jAmQh!l`dlDolYY8-l7ng5m-SbGmz7 z44N9nFSo{o#1MJo;|=zwAUudO%yjXuR8o)D(idg)oLq95htT!Ji>bW~y~$N6c$RxX$#k;(w-iXq@Ae&+a*xjc`l|iCimSGlp%iNo zmj%`H0P#WN3-4Y}mr+V7)im&1D8-{wa=#<^7t(cByX);opz)k*;UC3quL~auEb4Dl zT8W&D=)Uv(kGprIXeCG~98fOQqBoytmhDzK)|^ZSnsoSydTxc*kzh}w&9oYs-BI^a zEccy<@w;P`+MdpX;7H>#+JCV3(b3E~J&o67TF}BN65#E%=;WdLvuEMMj|@=G@{dDt z7s6i(_s=}DAT6RH?&c*ky_O0Ocuh4s@m0V-oOQ8qpvR6G9r_vpQ-Xn1^1rHIdE#T$ zNdSM|*$f}QGq@uJE&QmM;VK4iP#ZP~q}8+@>&_WhvDa%+3KqE^Ej{*I0#9i?y$G-T kuY(Tel-8&otZ%#hAJepWR?y0d2mk;807*qoM6N<$f{w&dzW@LL literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html b/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html new file mode 100644 index 0000000000..3b4b352bb3 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-01-07 21:47:14Functions: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..06cb69bdc5 --- /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-01-07 21:47:14Functions: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..c6d789883b --- /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-01-07 21:47:14Functions: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          55 :       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       94931 :       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          59 :       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-01-07 21:47:14Functions: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..0e53897ee3 --- /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-01-07 21:47:14Functions: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..3d6dbb0d97 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.html @@ -0,0 +1,328 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-01-07 21:47:14Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/timer.h.gcov.png b/mrs_lib/include/mrs_lib/timer.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ce5e63f494d2e725baaa09e6f85316b2254598fa GIT binary patch literal 896 zcmV-`1AqL9P)(9B=S(B-K36%Z;|fgIa;!!G+K=VWZfW^vB2bc0kyqgH4RWh zpdf&O2l)ymcL1o(DIpZWSTv$Y)V!dwk@v&nBRdJ$xq$2^30m_Q>pSD+L4|P_C3Ol# z64EK?d*MTyJQlPkvg|0L2k;|}&R_@*!*~3_Jojb}1381qu?&IWIy5Gba~_X1`t#T_ zWZ5rb1aQH{O^`@bQOv>y9lP>K90Ntso-1e4GX-b2+vD+A9%(Ifxoxj3nAUwPn|?LlU9tJ6pZ+aw4zw}RSkB;<%`TnlpZn}cyMZtZJFFR5^&Hd(sZk`NAvA0ajZ=vB3z&@A@W|D>67pOz}X_&oXR+z=n-!Us3okOd}YgV{sg?_2@Ou?C+ z73w6gJuBRo^!Y-6`Le9v2rZl8C~D#P+!ha`-Wn>#f=AX(%bsJ6*H??2Vr(iNc)02Fstn$DDg=c3vZc_tfm2K zvxiYf(?R98W-hB~5D;+7o_-G8=#+fFEEYWT5l6|{O9}sxxe$a9V%^SFX#Po)X-P7^ zErWeWBpm!nS5YI=)Efx8q?=`yBAjm>!i&i6CMWD + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-01-07 21:47:14Functions: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&)183
mrs_lib::Transformer::retryLookupNewest(bool)229
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1006
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)231011
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1303751
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1303764
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1306622
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1308124
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&)1309246
+
+
+ + + +
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..3fb1078428 --- /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-01-07 21:47:14Functions: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&)1303751
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1306622
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1006
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)231011
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&)1309246
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)183
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)229
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&)1303764
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1308124
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..e07835e404 --- /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-01-07 21:47:14Functions: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      231011 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132      399791 :       std::scoped_lock lck(mutex_);
+     133      231002 :       default_frame_id_ = frame_id;
+     134      230996 :     }
+     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         183 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155         309 :       std::scoped_lock lck(mutex_);
+     156         183 :       if (prefix.empty())
+     157          58 :         prefix_ = "";
+     158             :       else
+     159         182 :         prefix_ = prefix + "/";
+     160         183 :     }
+     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         286 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213         343 :       std::scoped_lock lck(mutex_);
+     214         286 :       retry_lookup_newest_ = retry;
+     215         229 :     }
+     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        1006 :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250        2006 :       std::scoped_lock lck(mutex_);
+     251        2012 :       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     1306383 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552     1306383 :       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     1309262 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565     1309262 :       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     1306396 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576     1306396 :       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     1310764 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589     1308124 :       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     1309246 :     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     1309246 :       geometry_msgs::TransformStamped ret;
+     621     1309069 :       frame_from(ret) = from_frame;
+     622     1309290 :       frame_to(ret) = to_frame;
+     623     1309198 :       ret.header.stamp = time_stamp;
+     624     1309198 :       ret.transform = tf;
+     625     1309198 :       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-01-07 21:47:14Functions: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..19da8032a2 --- /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-01-07 21:47:14Functions: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..112d634f8d --- /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-01-07 21:47:14Functions: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..fac24f6eb1 --- /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-01-07 21:47:14Functions: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)32748
+
+
+ + + +
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..680befb0ef --- /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-01-07 21:47:14Functions: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)32748
+
+
+ + + +
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..8da72840b8 --- /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-01-07 21:47:14Functions: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       32748 :   int signum(T val)
+     139             :   {
+     140       32748 :     return (T(0) < val) - (val < T(0));
+     141             :   }
+     142             : 
+     143             : }  // namespace mrs_lib
+     144             : 
+     145             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/utils.h.gcov.overview.html b/mrs_lib/include/mrs_lib/utils.h.gcov.overview.html new file mode 100644 index 0000000000..d983ecb00f --- /dev/null +++ b/mrs_lib/include/mrs_lib/utils.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/utils.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.png b/mrs_lib/include/mrs_lib/visual_object.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e5a217429afe8a62a7258891b3370af79723c76a GIT binary patch literal 484 zcmV_%GNWv0>g zGt_r9t5M~ynG$$UJ_W8188j)if_*u)woji{4E;^-{UY~_0nSSu_-j7Nl$zcH=Mcti z)LgHKKcR-lN5XuYPz`FHO2WO58WLXINgWlGxZkP6ZEO*qtF;Z}`~XH;uSyM>+#9J& z_6GG-tp`#ZSfnJqHx)P_c9HB8iB{~xc}6NHdtoC-LT$~lM;Ie{9)mn+hs+wZj;yw~ z2H4EHsB!qhXOsUFX`cCk=0ilxm(p|t`4kb#faQ^kN(r;d_&|oJg?}!bGq6F1RhxzO aNB#jj9b7t&#$cTQ0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-01-07 21:47:14Functions: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&)16210
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)63037
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()74366
mrs_lib::AttitudeConverter::getYaw()108397
mrs_lib::AttitudeConverter::calculateRPY()108402
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)153097
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)169305
mrs_lib::AttitudeConverter::setHeading(double const&)178017
mrs_lib::AttitudeConverter::getVectorZ()180014
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const229762
mrs_lib::AttitudeConverter::operator tf2::Transform() const236196
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)265717
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const349332
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const521485
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)554895
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)718655
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const881452
mrs_lib::AttitudeConverter::getHeading()935048
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)2085855
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)4432719
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const5138536
mrs_lib::AttitudeConverter::validateOrientation()8056991
+
+
+ + + +
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..b21e938fa2 --- /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-01-07 21:47:14Functions: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&)153097
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)16210
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()935048
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()180014
mrs_lib::AttitudeConverter::setHeading(double const&)178017
mrs_lib::AttitudeConverter::calculateRPY()108402
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)169305
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)63037
mrs_lib::AttitudeConverter::validateOrientation()8056991
mrs_lib::AttitudeConverter::getYaw()108397
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> >)2085855
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)554895
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)265717
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)718655
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&)4432719
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()74366
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>() const349332
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const5138536
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const521485
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const229762
mrs_lib::AttitudeConverter::operator tf2::Transform() const236196
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const881452
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..4a7685d799 --- /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-01-07 21:47:14Functions: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       16210 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34       16210 :   vector3_[0] = vector3[0];
+      35       16210 :   vector3_[1] = vector3[1];
+      36       16210 :   vector3_[2] = vector3[2];
+      37       16210 : }
+      38             : 
+      39      153097 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41      153079 :   vector3_[0] = vector3.x;
+      42      153082 :   vector3_[1] = vector3.y;
+      43      153058 :   vector3_[2] = vector3.z;
+      44      153070 : }
+      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      349332 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60      349332 :   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     4432719 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80     4431774 :   switch (format) {
+      81             : 
+      82     4431729 :     case RPY_EXTRINSIC: {
+      83     4431729 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84     4431833 :       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     4431868 :   validateOrientation();
+     104     4432230 : }
+     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     2085855 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117     2085611 :   tf2_quaternion_.setX(quaternion.x);
+     118     2085237 :   tf2_quaternion_.setY(quaternion.y);
+     119     2085615 :   tf2_quaternion_.setZ(quaternion.z);
+     120     2085849 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122     2085894 :   validateOrientation();
+     123     2085446 : }
+     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      265717 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132      265717 :   tf2_quaternion_.setX(quaternion.x());
+     133      265716 :   tf2_quaternion_.setY(quaternion.y());
+     134      265717 :   tf2_quaternion_.setZ(quaternion.z());
+     135      265717 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137      265716 :   validateOrientation();
+     138      265716 : }
+     139             : 
+     140      718655 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141      717086 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143      717944 :   tf2_quaternion_.setX(quaternion.x());
+     144      717307 :   tf2_quaternion_.setY(quaternion.y());
+     145      717892 :   tf2_quaternion_.setZ(quaternion.z());
+     146      717787 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148      717878 :   validateOrientation();
+     149      716585 : }
+     150             : 
+     151      554895 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153      554288 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155      554288 :   validateOrientation();
+     156      554230 : }
+     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      521485 : AttitudeConverter::operator tf2::Quaternion() const {
+     170      521485 :   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     5138536 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187     5138536 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189     5137813 :   geom_quaternion.x = tf2_quaternion_.x();
+     190     5134944 :   geom_quaternion.y = tf2_quaternion_.y();
+     191     5134655 :   geom_quaternion.z = tf2_quaternion_.z();
+     192     5135474 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194     5135816 :   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      881452 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207      881452 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209     1762488 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212       74366 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214       74366 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216       74366 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219      229762 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221      229762 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224      236196 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226      236196 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233      108397 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235      108397 :   calculateRPY();
+     236             : 
+     237      108397 :   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      935048 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256      935048 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258      934833 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260      934540 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           4 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264     1868056 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267       63037 : 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       63037 :   if (fabs(heading_rate) < 1e-3) {
+     272       40816 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276       22221 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279       22221 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280       22221 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283       22221 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284       22221 :   b_orb.normalize();
+     285       22221 :   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       22221 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291       22221 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292       22221 :   double projected_norm        = projected.norm();
+     293             : 
+     294       22221 :   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       22221 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301       22221 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303       22221 :   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       22221 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312      169305 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315      169305 :   Eigen::Matrix3d R = *this;
+     316      169281 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319      169239 :   Eigen::Matrix3d W;
+     320      169248 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323      169238 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326      169257 :   double rx = R(0, 0);  // x-component of body X
+     327      169183 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329      169229 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331      169229 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336      169229 :   double atan2_d_x = -ry / denom;
+     337      169229 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340      169229 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342      169249 :   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      180014 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365      180014 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367      180016 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369      360031 :   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      178017 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406      178017 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409      178015 :   if (fabs(b3[2]) < 1e-3) {
+     410           2 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414      178012 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416      178016 :   Eigen::Matrix3d new_R;
+     417             : 
+     418      178012 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421      178012 :   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      355400 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425      177881 :   A.col(0)          = projector_body_z_compl.col(0);
+     426      177866 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429      355224 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430      177907 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431      177836 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434      354979 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435      355233 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436      177589 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438      177381 :   new_R.col(0) = oblique_projector * h;
+     439      177128 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443      177289 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444      176409 :   new_R.col(1).normalize();
+     445             : 
+     446      354556 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455      108402 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457      108402 :   if (!got_rpy_) {
+     458             : 
+     459      108402 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461      108402 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467     8056991 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469    16109476 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470     8050344 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473     8051855 : }
+     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..a407b2754c --- /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-01-07 21:47:14Functions: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..f2619db475 --- /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-01-07 21:47:14Functions: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..8b942d7881 --- /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-01-07 21:47:14Functions: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..959cf85d40 --- /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-01-07 21:47:14Functions: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..a99e0156eb --- /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-01-07 21:47:14Functions: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..475130ab03 --- /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-01-07 21:47:14Functions: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..0dc11d0512 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221376.1 %
Date:2024-01-07 21:47:14Functions:132259.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::BatchVisualizer::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addTrajectory(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setLinesScale(double)0
mrs_lib::BatchVisualizer::addRay(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCone(mrs_lib::geometry::Cone const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCuboid(mrs_lib::geometry::Cuboid const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::setParentFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)6
mrs_lib::BatchVisualizer::setPointsScale(double)6
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)80
mrs_lib::BatchVisualizer::initialize()114
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> >)114
mrs_lib::BatchVisualizer::BatchVisualizer()114
mrs_lib::BatchVisualizer::clearBuffers()120
mrs_lib::BatchVisualizer::clearVisuals()120
mrs_lib::BatchVisualizer::~BatchVisualizer()228
mrs_lib::BatchVisualizer::addNullPoint()234
mrs_lib::BatchVisualizer::addNullLine()240
mrs_lib::BatchVisualizer::addNullTriangle()240
mrs_lib::BatchVisualizer::publish()240
+
+
+ + + +
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..f5ef2c035b --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221376.1 %
Date:2024-01-07 21:47:14Functions:132259.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::BatchVisualizer::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::initialize()114
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullLine()240
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullPoint()234
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::clearBuffers()120
mrs_lib::BatchVisualizer::clearVisuals()120
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> >)6
mrs_lib::BatchVisualizer::setPointsScale(double)6
mrs_lib::BatchVisualizer::addNullTriangle()240
mrs_lib::BatchVisualizer::addRay(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&)0
mrs_lib::BatchVisualizer::addCone(mrs_lib::geometry::Cone const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::publish()240
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)80
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> >)114
mrs_lib::BatchVisualizer::BatchVisualizer()114
mrs_lib::BatchVisualizer::~BatchVisualizer()228
+
+
+ + + +
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..86816364b7 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html @@ -0,0 +1,432 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221376.1 %
Date:2024-01-07 21:47:14Functions:132259.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..83593fe269cd764b2c7f8a49c73950ce03af9ca5 GIT binary patch literal 1246 zcmV<41R?v0P)4FDM+LWy#*X4g@_z#U54m6z$Bm9mkbWCO(C2m zybIVcEp20v*^5gna>5y}fy^ji#Vz4_2{cF7{0jn%yE$Mykgl+Wi$G=+>zHZA>oo#l zQG+t`k|pw<5=aM&e)X{W`9wYF88844`hn4mkOdS44{YlPwvo^eZW`&JA^_BF%*&8^ zj4Vi1+z%ub+3!rux)=Llai89iDi1Wh(awP63DQk@k0ft}8R-(Yi|4>>yLnvF6yO|{ z%bG~639rQa0gv}g0r-jQS&m~rijsZR;dShKlI40PEe!SSv8r9$m#gwPN?Cp$yBcj~r@F)lrkVjp8cB%x|!ji>3W*;bz2#foJc$83yUE(zS zis-ST(vv6U@ljVUK_o@y3e72i%l%tEBM>cZb z6&>02xF$#TQ^w2_+W4o&br0dZ?XI2?#fm$C!h+S+F`25wWRI`)5ps#Q8I-SA*i;~~jRHH$PnY&WarUs6f zCY?D={(4_a7O$$lCMHsY^oW7YUhDu4w6WRF4&b0^vd#{m)U;8>93Fy>pX@&Sj7JoO za(CB@r=ylNSBlfgX*%sBrDK3+crF%&sf)u2)af2oE7+g4y36j`=BE>w?yJ+zs8A*2 zxa*m^8*?T5Z9kBvIx{VaDt0k7ySS6oyH!_^1AvuU-4A5@+Pq1UfZ7?-9j=YBo&CV; zoSCFg`CCZ{I(w2y!bFP8pL9{hqbAn`QfZbVeD<;fxU$a`S2Z%cPr{C=uL|^}^Okv` zaMd**Fh1y`f}e!3eoH*EZ5-mUvA}@wNC1Te5iYUHRREOvd4i(qZK#&xV!Z literal 0 HcmV?d00001 diff --git a/mrs_lib/src/batch_visualizer/index-detail-sort-f.html b/mrs_lib/src/batch_visualizer/index-detail-sort-f.html new file mode 100644 index 0000000000..5d07dafdce --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19240847.1 %
Date:2024-01-07 21:47:14Functions:204346.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
<unnamed>15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
<unnamed>76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-detail-sort-l.html b/mrs_lib/src/batch_visualizer/index-detail-sort-l.html new file mode 100644 index 0000000000..bf97acc259 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19240847.1 %
Date:2024-01-07 21:47:14Functions:204346.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
<unnamed>15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
<unnamed>76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-detail.html b/mrs_lib/src/batch_visualizer/index-detail.html new file mode 100644 index 0000000000..70a1558eb6 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19240847.1 %
Date:2024-01-07 21:47:14Functions:204346.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
<unnamed>76.1 %162 / 21359.1 %13 / 22
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
<unnamed>15.4 %30 / 19533.3 %7 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-sort-f.html b/mrs_lib/src/batch_visualizer/index-sort-f.html new file mode 100644 index 0000000000..27dc2e3622 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19240847.1 %
Date:2024-01-07 21:47:14Functions:204346.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index-sort-l.html b/mrs_lib/src/batch_visualizer/index-sort-l.html new file mode 100644 index 0000000000..5f1ab066c5 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19240847.1 %
Date:2024-01-07 21:47:14Functions:204346.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/index.html b/mrs_lib/src/batch_visualizer/index.html new file mode 100644 index 0000000000..713cb5cb4a --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19240847.1 %
Date:2024-01-07 21:47:14Functions:204346.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
batch_visualizer.cpp +
76.1%76.1%
+ 		76.1 %162 / 21359.1 %13 / 22
visual_object.cpp +
15.4%15.4%
+ 		15.4 %30 / 19533.3 %7 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html new file mode 100644 index 0000000000..ce68ceac17 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3019515.4 %
Date:2024-01-07 21:47:14Functions:72133.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::msgToEigen(geometry_msgs::Point_<std::allocator<void> > const&)0
mrs_lib::VisualObject::addEllipse(mrs_lib::geometry::Ellipse const&, double, double, double, double, bool, int)0
mrs_lib::VisualObject::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool)0
mrs_lib::VisualObject::addRay(mrs_lib::geometry::Ray const&, double, double, double, double)0
mrs_lib::VisualObject::VisualObject(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cone const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cuboid const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ellipse const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cylinder const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Triangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Rectangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::buildEllipse(mrs_lib::geometry::Ellipse const&, int)0
mrs_lib::VisualObject::getID() const0
mrs_lib::eigenToMsg(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)80
mrs_lib::VisualObject::VisualObject(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&, unsigned long const&)80
mrs_lib::generateColor(double, double, double, double)80
mrs_lib::VisualObject::isTimedOut() const80
mrs_lib::VisualObject::getType() const80
mrs_lib::VisualObject::getColors() const80
mrs_lib::VisualObject::getPoints() const80
+
+
+ + + +
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..eb7f3078f5 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3019515.4 %
Date:2024-01-07 21:47:14Functions:72133.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::eigenToMsg(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)80
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&)80
mrs_lib::VisualObject::VisualObject(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ray const&, double, double, double, double, ros::Duration const&, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cone const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cuboid const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Ellipse const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Cylinder const&, double, double, double, double, ros::Duration const&, bool, bool, unsigned long const&, int)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Triangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::VisualObject::VisualObject(mrs_lib::geometry::Rectangle const&, double, double, double, double, ros::Duration const&, bool, unsigned long const&)0
mrs_lib::buildEllipse(mrs_lib::geometry::Ellipse const&, int)0
mrs_lib::generateColor(double, double, double, double)80
mrs_lib::VisualObject::isTimedOut() const80
mrs_lib::VisualObject::getID() const0
mrs_lib::VisualObject::getType() const80
mrs_lib::VisualObject::getColors() const80
mrs_lib::VisualObject::getPoints() const80
+
+
+ + + +
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..99ad7ee07b --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html @@ -0,0 +1,401 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3019515.4 %
Date:2024-01-07 21:47:14Functions:72133.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #include <mrs_lib/visual_object.h>
+       2             : 
+       3             : namespace mrs_lib
+       4             : {
+       5             : 
+       6             : /* utils //{ */
+       7             : 
+       8             : /* conversions //{ */
+       9          80 : geometry_msgs::Point eigenToMsg(const Eigen::Vector3d& v) {
+      10          80 :   geometry_msgs::Point p;
+      11          80 :   p.x = v.x();
+      12          80 :   p.y = v.y();
+      13          80 :   p.z = v.z();
+      14          80 :   return p;
+      15             : }
+      16             : 
+      17          80 : std_msgs::ColorRGBA generateColor(const double r, const double g, const double b, const double a) {
+      18          80 :   std_msgs::ColorRGBA c;
+      19          80 :   c.r = r;
+      20          80 :   c.g = g;
+      21          80 :   c.b = b;
+      22          80 :   c.a = a;
+      23          80 :   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          80 : VisualObject::VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     116          80 :                            const unsigned long& id)
+     117          80 :     : id_(id) {
+     118          80 :   type_ = MarkerType::POINT;
+     119          80 :   points_.push_back(eigenToMsg(point));
+     120          80 :   colors_.push_back(generateColor(r, g, b, a));
+     121          80 :   if (timeout.toSec() <= 0) {
+     122          80 :     timeout_time_ = ros::Time(0);
+     123             :   } else {
+     124           0 :     timeout_time_ = ros::Time::now() + timeout;
+     125             :   }
+     126          80 : }
+     127             : //}
+     128             : 
+     129             : /* mrs_lib::geometry::Ray //{ */
+     130           0 : VisualObject::VisualObject(const mrs_lib::geometry::Ray& ray, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     131           0 :                            const unsigned long& id)
+     132           0 :     : id_(id) {
+     133           0 :   type_ = MarkerType::LINE;
+     134           0 :   addRay(ray, r, g, b, a);
+     135           0 :   if (timeout.toSec() <= 0) {
+     136           0 :     timeout_time_ = ros::Time(0);
+     137             :   } else {
+     138           0 :     timeout_time_ = ros::Time::now() + timeout;
+     139             :   }
+     140           0 : }
+     141             : //}
+     142             : 
+     143             : /* mrs_lib::geometry::Triangle //{ */
+     144           0 : VisualObject::VisualObject(const mrs_lib::geometry::Triangle& triangle, const double r, const double g, const double b, const double a,
+     145           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     146           0 :     : id_(id) {
+     147           0 :   addTriangle(triangle, r, g, b, a, filled);
+     148           0 :   if (timeout.toSec() <= 0) {
+     149           0 :     timeout_time_ = ros::Time(0);
+     150             :   } else {
+     151           0 :     timeout_time_ = ros::Time::now() + timeout;
+     152             :   }
+     153           0 : }
+     154             : //}
+     155             : 
+     156             : /* mrs_lib::geometry::Rectangle //{ */
+     157           0 : VisualObject::VisualObject(const mrs_lib::geometry::Rectangle& rectangle, const double r, const double g, const double b, const double a,
+     158           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     159           0 :     : id_(id) {
+     160           0 :   for (const auto& t : rectangle.triangles()) {
+     161           0 :     addTriangle(t, r, g, b, a, filled);
+     162             :   }
+     163           0 :   if (timeout.toSec() <= 0) {
+     164           0 :     timeout_time_ = ros::Time(0);
+     165             :   } else {
+     166           0 :     timeout_time_ = ros::Time::now() + timeout;
+     167             :   }
+     168           0 : }
+     169             : //}
+     170             : 
+     171             : /* mrs_lib::geometry::Cuboid //{ */
+     172           0 : VisualObject::VisualObject(const mrs_lib::geometry::Cuboid& cuboid, const double r, const double g, const double b, const double a,
+     173           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     174           0 :     : id_(id) {
+     175             : 
+     176           0 :   for (int i = 0; i < 6; i++) {
+     177           0 :     for (const auto& t : cuboid.getRectangle(i).triangles()) {
+     178           0 :       addTriangle(t, r, g, b, a, filled);
+     179             :     }
+     180             :   }
+     181           0 :   if (timeout.toSec() <= 0) {
+     182           0 :     timeout_time_ = ros::Time(0);
+     183             :   } else {
+     184           0 :     timeout_time_ = ros::Time::now() + timeout;
+     185             :   }
+     186           0 : }
+     187             : //}
+     188             : 
+     189             : /* mrs_lib::geometry::Ellipse//{ */
+     190           0 : VisualObject::VisualObject(const mrs_lib::geometry::Ellipse& ellipse, const double r, const double g, const double b, const double a,
+     191           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id, const int num_points)
+     192           0 :     : id_(id) {
+     193           0 :   addEllipse(ellipse, r, g, b, a, filled, num_points);
+     194           0 :   if (timeout.toSec() <= 0) {
+     195           0 :     timeout_time_ = ros::Time(0);
+     196             :   } else {
+     197           0 :     timeout_time_ = ros::Time::now() + timeout;
+     198             :   }
+     199           0 : }
+     200             : //}
+     201             : 
+     202             : /* mrs_lib::geometry::Cylinder //{ */
+     203           0 : VisualObject::VisualObject(const mrs_lib::geometry::Cylinder& cylinder, const double r, const double g, const double b, const double a,
+     204           0 :                            const ros::Duration& timeout, const bool filled, const bool capped, const unsigned long& id, const int num_sides)
+     205           0 :     : id_(id) {
+     206           0 :   if (capped) {
+     207           0 :     mrs_lib::geometry::Ellipse top    = cylinder.getCap(mrs_lib::geometry::Cylinder::TOP);
+     208           0 :     mrs_lib::geometry::Ellipse bottom = cylinder.getCap(mrs_lib::geometry::Cylinder::BOTTOM);
+     209           0 :     addEllipse(top, r, g, b, a, filled, num_sides);
+     210           0 :     addEllipse(bottom, r, g, b, a, filled, num_sides);
+     211             :   }
+     212           0 :   std::vector<Eigen::Vector3d> top_points    = buildEllipse(cylinder.getCap(mrs_lib::geometry::Cylinder::TOP), num_sides);
+     213           0 :   std::vector<Eigen::Vector3d> bottom_points = buildEllipse(cylinder.getCap(mrs_lib::geometry::Cylinder::BOTTOM), num_sides);
+     214           0 :   for (unsigned int i = 0; i < top_points.size() - 1; i++) {
+     215           0 :     mrs_lib::geometry::Rectangle rect(bottom_points[i], bottom_points[i + 1], top_points[i + 1], top_points[i]);
+     216           0 :     addTriangle(rect.triangles()[0], r, g, b, a, filled);
+     217           0 :     addTriangle(rect.triangles()[1], r, g, b, a, filled);
+     218             :   }
+     219           0 :   mrs_lib::geometry::Rectangle rect(bottom_points[bottom_points.size() - 1], bottom_points[0], top_points[0], top_points[top_points.size() - 1]);
+     220           0 :   addTriangle(rect.triangles()[0], r, g, b, a, filled);
+     221           0 :   addTriangle(rect.triangles()[1], r, g, b, a, filled);
+     222           0 :   if (timeout.toSec() <= 0) {
+     223           0 :     timeout_time_ = ros::Time(0);
+     224             :   } else {
+     225           0 :     timeout_time_ = ros::Time::now() + timeout;
+     226             :   }
+     227           0 : }
+     228             : //}
+     229             : 
+     230             : /* mrs_lib::geometry::Cone //{ */
+     231           0 : VisualObject::VisualObject(const mrs_lib::geometry::Cone& cone, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     232           0 :                            const bool filled, const bool capped, const unsigned long& id, const int num_sides)
+     233           0 :     : id_(id) {
+     234           0 :   if (capped) {
+     235           0 :     mrs_lib::geometry::Ellipse cap = cone.getCap();
+     236           0 :     addEllipse(cap, r, g, b, a, filled, num_sides);
+     237             :   }
+     238           0 :   std::vector<Eigen::Vector3d> cap_points = buildEllipse(cone.getCap(), num_sides);
+     239           0 :   for (unsigned int i = 0; i < cap_points.size() - 1; i++) {
+     240           0 :     mrs_lib::geometry::Triangle tri(cap_points[i], cap_points[i + 1], cone.origin());
+     241           0 :     addTriangle(tri, r, g, b, a, filled);
+     242             :   }
+     243           0 :   mrs_lib::geometry::Triangle tri(cap_points[cap_points.size() - 1], cap_points[0], cone.origin());
+     244           0 :   addTriangle(tri, r, g, b, a, filled);
+     245           0 :   if (timeout.toSec() <= 0) {
+     246           0 :     timeout_time_ = ros::Time(0);
+     247             :   } else {
+     248           0 :     timeout_time_ = ros::Time::now() + timeout;
+     249             :   }
+     250           0 : }
+     251             : //}
+     252             : 
+     253             : /* mrs_msgs::TrajectoryReference //{ */
+     254           0 : VisualObject::VisualObject(const mrs_msgs::TrajectoryReference& traj, const double r, const double g, const double b, const double a,
+     255           0 :                            const ros::Duration& timeout, const bool filled, const unsigned long& id)
+     256           0 :     : id_(id) {
+     257           0 :   if (traj.points.size() < 2) {
+     258           0 :     return;
+     259             :   }
+     260           0 :   if (filled) {
+     261           0 :     for (size_t i = 0; i < traj.points.size() - 1; i++) {
+     262           0 :       Eigen::Vector3d p1, p2;
+     263           0 :       p1.x()   = traj.points[i].position.x;
+     264           0 :       p1.y()   = traj.points[i].position.y;
+     265           0 :       p1.z()   = traj.points[i].position.z;
+     266           0 :       p2.x()   = traj.points[i + 1].position.x;
+     267           0 :       p2.y()   = traj.points[i + 1].position.y;
+     268           0 :       p2.z()   = traj.points[i + 1].position.z;
+     269           0 :       auto ray = mrs_lib::geometry::Ray::twopointCast(p1, p2);
+     270           0 :       addRay(ray, r, g, b, a);
+     271             :     }
+     272             :   } else {
+     273           0 :     type_ = MarkerType::POINT;
+     274           0 :     for (size_t i = 0; i < traj.points.size(); i++) {
+     275           0 :       points_.push_back(traj.points[i].position);
+     276           0 :       colors_.push_back(generateColor(r, g, b, a));
+     277             :     }
+     278             :   }
+     279           0 :   if (timeout.toSec() <= 0) {
+     280           0 :     timeout_time_ = ros::Time(0);
+     281             :   } else {
+     282           0 :     timeout_time_ = ros::Time::now() + timeout;
+     283             :   }
+     284             : }
+     285             : //}
+     286             : 
+     287             : /* getID //{ */
+     288           0 : unsigned long VisualObject::getID() const {
+     289           0 :   return id_;
+     290             : }
+     291             : //}
+     292             : 
+     293             : /* getType //{ */
+     294          80 : int VisualObject::getType() const {
+     295          80 :   return type_;
+     296             : }
+     297             : //}
+     298             : 
+     299             : /* isTimedOut //{ */
+     300          80 : bool VisualObject::isTimedOut() const {
+     301          80 :   return !timeout_time_.isZero() && (timeout_time_ - ros::Time::now()).toSec() <= 0;
+     302             : }
+     303             : //}
+     304             : 
+     305             : /* getPoints //{ */
+     306          80 : const std::vector<geometry_msgs::Point> VisualObject::getPoints() const {
+     307          80 :   return points_;
+     308             : }
+     309             : //}
+     310             : 
+     311             : /* getColors //{ */
+     312          80 : const std::vector<std_msgs::ColorRGBA> VisualObject::getColors() const {
+     313          80 :   return colors_;
+     314             : }
+     315             : //}
+     316             : 
+     317             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html new file mode 100644 index 0000000000..b910f327db --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html @@ -0,0 +1,100 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..af38cbc0ca11fc9d3c74d943d142de04550935b1 GIT binary patch literal 1166 zcmV;91abR`P)0GscesKPK=aG>FH;9Jv{QJ-20V9)iEvnYI9TmjjMN#B7W1 z`m{ePt})Nh+_qPqYlP&P+1n=;XbdL_3v{xx4*{|y`5tqZM}*qf!f^;J*mwZXOapYc zI-1!N+mPP%=jEgZ7Mi9u9(xWh%^(#(OcA^vB%b`M}UQsl0E zEh2aAE9Yl`tPZaL_F^&DqsABhhEBjhD0-xY=y54R-w-~0t8Nu zktT6X5X}0V3Z!ioXV19UvFu3Mgl-^s)Ietpa2P(YSfEaa*vu%$o-iDXlJbOeEl>M> zQ)%X$98HsI)-3}gCz5#1$gxj3%W*{cFZs11PkEtM&ws4`ck{=kDNw_cmoMj0QEP$j z-ECOwy0UpI^8_*wH@0^MxVq63umu)AS7E4@nF!kix*li&tITi!CVND67n2NYjY2)E z&>cMn&YRijBZ9rnH0Kb4nhAllG!p}CU9@KLxO7hy?B@!fq5r4vOad$N$3(xMu+OBf z-)lX;AxMovm1I4okm^`ZLYQc2q2`*fo8HbPGZTO(%}mIWL^F$);|^DtnN~b?8g7K1W5P}>H?h``R#~n- z4_7Z3-WDzbPg?lbNeeFt*tK8R@^LK%eFMCWLI9pMieH*?P0WWkLkuKHqrheci34JD zq}501EeFWJIs(qDb76VIPNOsnJeYZnn5FoIKd9q9o}?i&nsrW-j%cchG7r1ZV;}gA zcrXbQ*SR#5nZ>3)@QytK@LcSX=B0>Ys&mK*m*Fz|l8{A*^y2!7*pvLL8!3h%<*i7; zOvj%N0M#ypWPRZlH`m-r3(qk_qO$kt$c&2e?XCmh*;JwLu|#L>W%k(1QiUXHx$qGu z{|a!f5LYiYVIQQGT2*rao$sVTlp8Sk>w%r1cO9^^fK%^%IVS(Jb^j)#bJ*u~IU9;&+wE;SsN@M?=+~elCG3y`S^{HXI+Eg5t9y`lfVLxtv zt*!voD`!{jbyaM gbNzfgRewzKA60I + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-01-07 21:47:14Functions: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&)63678
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)63678
+
+
+ + + +
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..cb84995c4b --- /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-01-07 21:47:14Functions: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&)63678
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&)63678
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..df6e51c2ef --- /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-01-07 21:47:14Functions: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       63678 :     geometry_msgs::Quaternion fromEigen(const Eigen::Quaterniond& what)
+      48             :     {
+      49       63678 :       geometry_msgs::Quaternion q;
+      50       63678 :       q.x = what.x();
+      51       63678 :       q.y = what.y();
+      52       63678 :       q.z = what.z();
+      53       63678 :       q.w = what.w();
+      54       63678 :       return q;
+      55             :     }
+      56             :     
+      57       63678 :     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       63678 :       Eigen::Quaterniond q;
+      61       63678 :       q.x() = what.x;
+      62       63678 :       q.y() = what.y;
+      63       63678 :       q.z() = what.z;
+      64       63678 :       q.w() = what.w;
+      65       63678 :       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..2b8db7c2e9 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6544514.6 %
Date:2024-01-07 21:47:14Functions:149414.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
<unnamed>51.6 %33 / 6443.8 %7 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
<unnamed>69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-detail-sort-l.html b/mrs_lib/src/geometry/index-detail-sort-l.html new file mode 100644 index 0000000000..89eebc1fe6 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6544514.6 %
Date:2024-01-07 21:47:14Functions:149414.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
<unnamed>51.6 %33 / 6443.8 %7 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
<unnamed>69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-detail.html b/mrs_lib/src/geometry/index-detail.html new file mode 100644 index 0000000000..613ca3dc4c --- /dev/null +++ b/mrs_lib/src/geometry/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6544514.6 %
Date:2024-01-07 21:47:14Functions:149414.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
<unnamed>69.6 %32 / 4670.0 %7 / 10
misc.cpp +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
<unnamed>51.6 %33 / 6443.8 %7 / 16
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-sort-f.html b/mrs_lib/src/geometry/index-sort-f.html new file mode 100644 index 0000000000..4667bdfe5c --- /dev/null +++ b/mrs_lib/src/geometry/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6544514.6 %
Date:2024-01-07 21:47:14Functions:149414.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index-sort-l.html b/mrs_lib/src/geometry/index-sort-l.html new file mode 100644 index 0000000000..cd56c0e442 --- /dev/null +++ b/mrs_lib/src/geometry/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6544514.6 %
Date:2024-01-07 21:47:14Functions:149414.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
misc.cpp +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/index.html b/mrs_lib/src/geometry/index.html new file mode 100644 index 0000000000..51096ae37c --- /dev/null +++ b/mrs_lib/src/geometry/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6544514.6 %
Date:2024-01-07 21:47:14Functions:149414.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
conversions.cpp +
69.6%69.6%
+ 		69.6 %32 / 4670.0 %7 / 10
misc.cpp +
51.6%51.6%
+ 		51.6 %33 / 6443.8 %7 / 16
shapes.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 68
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.func-sort-c.html b/mrs_lib/src/geometry/misc.cpp.func-sort-c.html new file mode 100644 index 0000000000..fa6e2f6922 --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func-sort-c.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:336451.6 %
Date:2024-01-07 21:47:14Functions:71643.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::triangleArea(double, double, double)0
mrs_lib::geometry::quaternionBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)0
mrs_lib::geometry::quaternionFromEuler(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::geometry::quaternionFromEuler(double, double, double)0
mrs_lib::geometry::sphericalTriangleArea(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_lib::geometry::haversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::rotationBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::angleaxisBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::cross(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>)3
mrs_lib::geometry::quaternionFromHeading(double)63684
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)94562
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)127916
+
+
+ + + +
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..67267f1a5b --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:336451.6 %
Date:2024-01-07 21:47:14Functions:71643.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)94562
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)63684
mrs_lib::geometry::sphericalTriangleArea(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)0
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)127916
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..542b62d213 --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.html @@ -0,0 +1,269 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:336451.6 %
Date:2024-01-07 21:47:14Functions:71643.8 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : #include <mrs_lib/geometry/misc.h>
+       3             : 
+       4             : namespace mrs_lib
+       5             : {
+       6             :   namespace geometry
+       7             :   {
+       8             : 
+       9             :     // instantiation of common template values
+      10             :     vec_t<3 + 1> toHomogenous(const vec_t<3>& vec);
+      11             :     vec_t<2 + 1> toHomogenous(const vec_t<2>& vec);
+      12             : 
+      13             :     // | ----------------- Angle-related functions ---------------- |
+      14             : 
+      15             :     /* angle-related functions //{ */
+      16             : 
+      17             :     /* cross() //{ */
+      18             : 
+      19           3 :     double cross(const vec2_t& vec1, const vec2_t vec2)
+      20             :     {
+      21           3 :       return vec1.x() * vec2.y() - vec1.y() * vec2.x();
+      22             :     }
+      23             : 
+      24             :     //}
+      25             : 
+      26             :     /* angleBetween() //{ */
+      27             : 
+      28       94562 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30       94562 :       const double sin_12 = vec1.cross(vec2).norm();
+      31       94562 :       const double cos_12 = vec1.dot(vec2);
+      32       94563 :       const double angle = std::atan2(sin_12, cos_12);
+      33       94563 :       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       63684 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94      127368 :       return quat_t(anax_t(heading, Eigen::Vector3d::UnitZ()));
+      95             :     }
+      96             :     //}
+      97             : 
+      98             :     //}
+      99             : 
+     100             :     /* rotationBetween() //{ */
+     101             : 
+     102           3 :     Eigen::Matrix3d rotationBetween(const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2, const double tolerance)
+     103             :     {
+     104           3 :       const auto rot = angleaxisBetween(vec1, vec2, tolerance);
+     105           3 :       const Eigen::Matrix3d ret(rot);
+     106           6 :       return ret;
+     107             :     }
+     108             : 
+     109             :     //}
+     110             : 
+     111             :     /* haversin() //{ */
+     112             : 
+     113           0 :     double haversin(const double angle)
+     114             :     {
+     115           0 :       return (1.0 - std::cos(angle)) / 2.0;
+     116             :     }
+     117             : 
+     118             :     //}
+     119             : 
+     120             :     /* invHaversin() //{ */
+     121             : 
+     122           0 :     double invHaversin(const double value)
+     123             :     {
+     124           0 :       return 2.0 * std::asin(std::sqrt(value));
+     125             :     }
+     126             : 
+     127             :     //}
+     128             : 
+     129             :     /* solidAngle() //{ */
+     130           0 :     double solidAngle(double a, double b, double c)
+     131             :     {
+     132           0 :       return invHaversin((haversin(c) - haversin(a - b)) / (std::sin(a) * std::sin(b)));
+     133             :     }
+     134             :     //}
+     135             : 
+     136             :     //}
+     137             : 
+     138             :     /* triangleArea() //{ */
+     139             : 
+     140           0 :     double triangleArea(const double a, const double b, const double c)
+     141             :     {
+     142           0 :       double s = (a + b + c) / 2.0;
+     143           0 :       return std::sqrt(s * (s - a) * (s - b) * (s - c));
+     144             :     }
+     145             : 
+     146             :     //}
+     147             : 
+     148             :     /* sphericalTriangleArea //{ */
+     149           0 :     double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c)
+     150             :     {
+     151           0 :       double ab = angleBetween(a, b);
+     152           0 :       double bc = angleBetween(b, c);
+     153           0 :       double ca = angleBetween(c, a);
+     154             : 
+     155           0 :       if (ab < 1e-3 and bc < 1e-3 and ca < 1e-3)
+     156             :       {
+     157           0 :         return triangleArea(ab, bc, ca);
+     158             :       }
+     159             : 
+     160           0 :       double A = solidAngle(ca, ab, bc);
+     161           0 :       double B = solidAngle(ab, bc, ca);
+     162           0 :       double C = solidAngle(bc, ca, ab);
+     163             : 
+     164           0 :       return A + B + C - M_PI;
+     165             :     }
+     166             :     //}
+     167             : 
+     168             :     /* vector distance //{ */
+     169             : 
+     170           0 :     double dist(const vec2_t& a, const vec2_t& b)
+     171             :     {
+     172             : 
+     173           0 :       return (a - b).norm();
+     174             :     }
+     175             : 
+     176      127916 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179      127916 :       return (a - b).norm();
+     180             :     }
+     181             : 
+     182             :     //}
+     183             : 
+     184             :   }  // namespace geometry
+     185             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.overview.html b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html new file mode 100644 index 0000000000..bdcd7dc00e --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html @@ -0,0 +1,67 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.png b/mrs_lib/src/geometry/misc.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8118e0b3b14707468e51b536c53b3fd75d30c55a GIT binary patch literal 760 zcmVi;~Z+NCa7s z+`}tGDv?9e60itJR2Ge^S6M%{VTVw|6uq!RendT&)SOb2s(}qQI&B19>_ghFI?_X) z6RT7W(WGTv8Dkq`=DgWs7srZ30E7Q8MF6eJeOGGzZKuW-dWJ7u>q092M(9I?Gexta z+TV2&zH6pi)<~akQ_0g~vT1@dEf>9%shPsHh2jZ5U)YUFpY7 z^avxVt{SSy>q;-nle{OUR(Thy0Z@2d`nn5PT0NQk`4CvO9!qR0ps60)^}rgq;;8bH zvvKqMxDbmq9zsrlQwcwjz=1^q&+{ltV52wkipM&7-W9H6It$5n;1PQd_Ru1LP236ElC+?{%CJiJQh6EIX7SMfH_^e+m$DxOXz zoe-MTN8@>djMqoYtfxldT^Po-klA)U&Ed@rdt6?_($td8CeSmH@G<)-MPtNYu2X66 zNH8t33c;>l!>xMo<)ZoL-}i(xTE~RfF3)fdLN3xt;zEf=r!xY+ zC=^QhCv7xQP-q`C_PB;e@Cx!@oCB=4Q3WQPfG$ZcmB6ZL>-{zE(jmX0s@GU8hh5 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/shapes.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - shapes.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2024-01-07 21:47:14Functions: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..8af9ea9ef8 --- /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-01-07 21:47:14Functions: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..c5674e83ee --- /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-01-07 21:47:14Functions: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-01-07 21:47:14Functions: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..bcf404b729 --- /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-01-07 21:47:14Functions: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..fb2a6edbb6 --- /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-01-07 21:47:14Functions: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..321f65bb3f --- /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-01-07 21:47:14Functions: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..297d14edc5 --- /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-01-07 21:47:14Functions: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..25c79a168e --- /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-01-07 21:47:14Functions: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..4e1d06af61 --- /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-01-07 21:47:14Functions: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..c06688d70f --- /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-01-07 21:47:14Functions: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..8156f19ca6 --- /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-01-07 21:47:14Functions: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-01-07 21:47:14Functions: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..1f6232c2d9 --- /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-01-07 21:47:14Functions: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..3ad5e7632a --- /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-01-07 21:47:14Functions: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..05dd9221ab --- /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-01-07 21:47:14Functions: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..2369799748 --- /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-01-07 21:47:14Functions: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..70240f418a --- /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-01-07 21:47:14Functions: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..e847b1b21c --- /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-01-07 21:47:14Functions: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&&)55
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)56
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)58
mrs_lib::MedianFilter::check(double)89240
mrs_lib::MedianFilter::median() const89260
mrs_lib::MedianFilter::full() const94582
mrs_lib::MedianFilter::add(double)94591
+
+
+ + + +
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..6e2d63a50d --- /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-01-07 21:47:14Functions: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)94591
mrs_lib::MedianFilter::check(double)89240
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)55
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)58
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)56
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const94582
mrs_lib::MedianFilter::median() const89260
+
+
+ + + +
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..916fe4d783 --- /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-01-07 21:47:14Functions: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          58 :   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          58 :       m_max_diff(max_diff)
+      12             :   {
+      13          58 :     m_buffer.set_capacity(buffer_length);
+      14          58 :     m_buffer_sorted.reserve(buffer_length);
+      15          58 :   }
+      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          55 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33          55 :     *this = other;
+      34          55 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39          56 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41          56 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43          56 :     m_buffer = other.m_buffer;
+      44          56 :     m_buffer_sorted = other.m_buffer_sorted;
+      45          56 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48          56 :     m_min_valid = other.m_min_valid;
+      49          56 :     m_max_valid = other.m_max_valid;
+      50          56 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52         112 :     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       94591 :   void MedianFilter::add(const double value)
+      74             :   {
+      75      189162 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77       94582 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79       94578 :     m_median = std::nullopt;
+      80       94580 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84       89240 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86       89240 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88       89241 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89      178475 :     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       94582 :   bool MedianFilter::full() const
+     113             :   {
+     114      189158 :     std::scoped_lock lck(m_mtx);
+     115      189154 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120       89260 :   double MedianFilter::median() const
+     121             :   {
+     122      178509 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124       89250 :     if (m_median.has_value())
+     125          19 :       return m_median.value();
+     126             :   
+     127             :     // check if there are even any numbers to calculate the median from
+     128       89234 :     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       89231 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137       89234 :     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       89231 :     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       89216 :     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       89218 :     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       89238 :     if (even_set)
+     148       89222 :       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       89236 :     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-01-07 21:47:14Functions: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..49efbc3904 --- /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-01-07 21:47:14Functions: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..cb17725eeb --- /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-01-07 21:47:14Functions: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..72b6c66182 --- /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-01-07 21:47:14Functions: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..9820196164 --- /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-01-07 21:47:14Functions: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..e8ede6bf08 --- /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-01-07 21:47:14Functions: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..542a713635 --- /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-01-07 21:47:14Functions: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)2512
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8142
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const54738
+
+
+ + + +
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..d2d865e51c --- /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-01-07 21:47:14Functions: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&)8142
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)2512
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const54738
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..37c16f9a7a --- /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-01-07 21:47:14Functions: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        2512 :   ParamProvider::ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam)
+      14        2512 :   : m_nh(nh), m_node_name(std::move(node_name)), m_use_rosparam(use_rosparam)
+      15             :   {
+      16        2512 :   }
+      17             : 
+      18        8142 :   bool ParamProvider::addYamlFile(const std::string& filepath)
+      19             :   {
+      20             :     try
+      21             :     {
+      22       16284 :       const auto loaded_yaml = YAML::LoadFile(filepath);
+      23        8142 :       YAML::Node root;
+      24        8142 :       root["root"] = loaded_yaml;
+      25        8142 :       m_yamls.emplace_back(root);
+      26        8142 :       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       54738 :   std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+      65             :   {
+      66      283665 :     for (const auto& yaml : m_yamls)
+      67             :     {
+      68             :       // Try to load the parameter sequentially as a map.
+      69      273935 :       auto cur_node_it = std::cbegin(yaml);
+      70             :       // The root should always be a pam
+      71      273937 :       if (!cur_node_it->second.IsMap())
+      72        8963 :         continue;
+      73             : 
+      74      264978 :       bool loaded = true;
+      75             :       {
+      76      264978 :         constexpr char delimiter = '/';
+      77      264978 :         auto substr_start = std::cbegin(param_name);
+      78      264978 :         auto substr_end = substr_start;
+      79      362596 :         do
+      80             :         {
+      81      627574 :           substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+      82             :           // why can't substr or string_view take iterators? :'(
+      83      627564 :           const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+      84      627564 :           const auto count = std::distance(substr_start, substr_end);
+      85      627564 :           const std::string param_substr = param_name.substr(start_pos, count);
+      86      627566 :           substr_start = substr_end+1;
+      87             : 
+      88      627564 :           bool found = false;
+      89     2566619 :           for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+      90             :           {
+      91     1311471 :             if (node_it->first.as<std::string>() == param_substr)
+      92             :             {
+      93      407607 :               cur_node_it = node_it;
+      94      407606 :               found = true;
+      95      407606 :               break;
+      96             :             }
+      97             :           }
+      98             : 
+      99      627571 :           if (!found)
+     100             :           {
+     101      219965 :             loaded = false;
+     102      219965 :             break;
+     103             :           }
+     104             :         }
+     105      407604 :         while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+     106             :       }
+     107             : 
+     108      264978 :       if (loaded)
+     109             :       {
+     110       90024 :         return cur_node_it->second;
+     111             :       }
+     112             :     }
+     113             : 
+     114        9726 :     return std::nullopt;
+     115             :   }
+     116             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html b/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html new file mode 100644 index 0000000000..af67093666 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html @@ -0,0 +1,49 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/index.html b/mrs_lib/src/profiler/index.html new file mode 100644 index 0000000000..7a33d33352 --- /dev/null +++ b/mrs_lib/src/profiler/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profilerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3910138.6 %
Date:2024-01-07 21:47:14Functions:91090.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
38.6%38.6%
+ 		38.6 %39 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html new file mode 100644 index 0000000000..20e1586679 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3910138.6 %
Date:2024-01-07 21:47:14Functions: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)799
mrs_lib::Profiler::Profiler()799
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)799
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)386415
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)386551
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)1062410
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)1062416
mrs_lib::Routine::end()1448904
mrs_lib::Routine::~Routine()1448953
+
+
+ + + +
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..04f4e4e326 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3910138.6 %
Date:2024-01-07 21:47:14Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.png b/mrs_lib/src/profiler/profiler.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ee7178374126c2cf561db47acab37502e0a548ac GIT binary patch literal 841 zcmV-P1GfB$P)#D`p{NA#Z`qPgMIV2pE;X_TZh;%%g;)Eg0W6tn(>_b{M%ptm^s zPsg)q+5y15pa>VDBIjUM^aDy3Z+Jb<$%>I;+AYgBinU)V3l>mZ4|j@0y2ULj(a#mgsjs=aZ}6&9Z8Pk6yah}WOJ1*I+E_4GKMZE z&}CCi@w`n$xYd|Kg= zHMueQ!@7o4b~b>R@`?G)fZwK(hXLPJiu(aK55p zgu2R#)zD5LxD@(Ku^O88_BIwh2T(x+GUw1WXB;Nsg=wU?UUf%mSD-QPk;k~^TB1O^ zTpyWaq1!kkO^AAn7Nvw{=bA+e!{b(>*djvO)(l_&vVP5G*FzglA7G^FjWa~AkFr-8 zo~>i6&=(S)ouu)sin{oCXWU6vz?ofULRo{~T4;5^AK1}+hLIvVbAMa&jLv`{I+KYd zzX_;s?EgQVNwaYgcbfRl^^%Vp&?Jy8$%n$34f(jdTwws2QSfvo-*t!Vh>j6)6n2_1 z&|EMv@4<@99%-%D?Wn@rG*1-YrYTwMeYb3Ach-I@rE5p!uB*@t|9ok0!0i+OZj4Ik zT%^SEi0dOL(shkXib_cq%k@!-{}w43`YlpzWL6cF3;Sm0itWD}9{NG`k0k#9NVwS- T>2taX00000NkvXXu0mjfhUbsa literal 0 HcmV?d00001 diff --git a/mrs_lib/src/safety_zone/index-detail-sort-f.html b/mrs_lib/src/safety_zone/index-detail-sort-f.html new file mode 100644 index 0000000000..31f7ef5eb0 --- /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-01-07 21:47:14Functions: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..6d33f1a0d9 --- /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-01-07 21:47:14Functions: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..9d2826c0f5 --- /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-01-07 21:47:14Functions: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..6bc9a85471 --- /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-01-07 21:47:14Functions: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..d2f53f5547 --- /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-01-07 21:47:14Functions: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..eb97632f57 --- /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-01-07 21:47:14Functions: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..460210292e --- /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-01-07 21:47:14Functions: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>)228
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)260
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>)260
+
+
+ + + +
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..5c648a686b --- /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-01-07 21:47:14Functions: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>)228
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)260
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>)260
+
+
+ + + +
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..e6ccc23bc2 --- /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-01-07 21:47:14Functions: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         228 : 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         228 :   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         260 : Intersection::Intersection(bool intersect, bool parallel, Eigen::RowVector2d point) : point(std::move(point)), parallel(parallel), intersect(intersect){}
+      16             : 
+      17             : /* sectionIntersect() //{ */
+      18             : 
+      19         260 : Intersection sectionIntersect(Eigen::RowVector2d start1, Eigen::RowVector2d end1, Eigen::RowVector2d start2, Eigen::RowVector2d end2) {
+      20         260 :   Eigen::RowVector2d vector1 = end1 - start1;
+      21         260 :   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         260 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+      29         260 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector2(0);
+      30             : 
+      31         260 :   if (cross == 0) {
+      32             :     // are parallel
+      33          32 :     if (difference_cross != 0)
+      34          32 :       return Intersection{false, true};
+      35             : 
+      36             :     // are collinear
+      37           0 :     double start2Scale = getScale(start1, vector1, start2);
+      38           0 :     if (0 <= start2Scale && start2Scale <= 1)
+      39           0 :       return Intersection{true, true, start2};
+      40           0 :     double end2Scale = getScale(start1, vector1, end2);
+      41           0 :     if (0 <= end2Scale && end2Scale <= 1)
+      42           0 :       return Intersection{true, true, end2};
+      43             : 
+      44           0 :     return Intersection{false, true};
+      45             :   }
+      46             : 
+      47         228 :   double             scale1 = difference_cross / cross;
+      48         228 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+      49         228 :   double             scale2 = getScale(start2, vector2, point);
+      50             : 
+      51         228 :   if (0 <= scale1 && scale1 <= 1 && 0 <= scale2 && scale2 <= 1) {
+      52           0 :     return Intersection{true, false, point};
+      53             :   }
+      54         228 :   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..83923f6449 --- /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-01-07 21:47:14Functions: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..d9122605a2 --- /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-01-07 21:47:14Functions: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..7d57fbc2e0 --- /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-01-07 21:47:14Functions: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..0362b20c0c --- /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-01-07 21:47:14Functions: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..7ca1a8a527 --- /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-01-07 21:47:14Functions: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..457de2d6cb --- /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-01-07 21:47:14Functions: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..d08c54a9ab --- /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-01-07 21:47:14Functions: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>)52
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)65
mrs_lib::Polygon::isPointInside(double, double)3253
mrs_lib::Polygon::getPointMessageVector(double)14100
+
+
+ + + +
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..02b9771f4d --- /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-01-07 21:47:14Functions: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)3253
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)65
mrs_lib::Polygon::getPointMessageVector(double)14100
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)52
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..30668ea1be --- /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-01-07 21:47:14Functions: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          52 : Polygon::Polygon(const Eigen::MatrixXd vertices) : vertices(vertices) {
+      10             : 
+      11          52 :   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          52 :   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          52 :   Eigen::RowVector2d edge1;
+      21          52 :   Eigen::RowVector2d edge2 = vertices.row(1) - vertices.row(0);
+      22         260 :   for (int i = 0; i < vertices.rows(); ++i) {
+      23         208 :     edge1 = edge2;
+      24         208 :     edge2 = vertices.row((i + 2) % vertices.rows()) - vertices.row((i + 1) % vertices.rows());
+      25             : 
+      26         208 :     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         208 :     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          52 : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* isPointInside() //{ */
+      40             : 
+      41        3253 : bool Polygon::isPointInside(const double px, const double py) {
+      42             : 
+      43        3253 :   int count = 0;
+      44             : 
+      45             :   // Cast a horizontal ray and see how many times it intersects all the edges
+      46       16265 :   for (int i = 0; i < vertices.rows(); ++i) {
+      47       13012 :     double v1x = vertices(i, 0);
+      48       13012 :     double v1y = vertices(i, 1);
+      49       13012 :     double v2x = vertices((i + 1) % vertices.rows(), 0);
+      50       13012 :     double v2y = vertices((i + 1) % vertices.rows(), 1);
+      51             : 
+      52       13012 :     if (v1y > py && v2y > py)
+      53        3177 :       continue;
+      54        9835 :     if (v1y <= py && v2y <= py)
+      55        3481 :       continue;
+      56        6354 :     double intersect_x    = (v1y == v2y) ? v1x : (py - v1y) * (v2x - v1x) / (v2y - v1y) + v1x;
+      57        6354 :     bool   does_intersect = intersect_x > px;
+      58        6354 :     if (does_intersect)
+      59        3177 :       ++count;
+      60             :   }
+      61             : 
+      62        3253 :   return count % 2;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* doesSectionIntersect() //{ */
+      68             : 
+      69          65 : bool Polygon::doesSectionIntersect(const double startX, const double startY, const double endX, const double endY) {
+      70             : 
+      71          65 :   Eigen::RowVector2d start{startX, startY};
+      72          65 :   Eigen::RowVector2d end{endX, endY};
+      73             : 
+      74         325 :   for (int i = 0; i < vertices.rows(); ++i) {
+      75             : 
+      76         260 :     Eigen::RowVector2d edgeStart = vertices.row(i);
+      77         260 :     Eigen::RowVector2d edgeEnd   = vertices.row((i + 1) % vertices.rows());
+      78             : 
+      79         260 :     if (sectionIntersect(start, end, edgeStart, edgeEnd).intersect) {
+      80           0 :       return true;
+      81             :     }
+      82             :   }
+      83             : 
+      84          65 :   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       14100 : std::vector<geometry_msgs::Point> Polygon::getPointMessageVector(const double z) {
+     194       14100 :   std::vector<geometry_msgs::Point> points(vertices.rows());
+     195       70500 :   for (int i = 0; i < vertices.rows(); ++i) {
+     196       56400 :     points[i].x = vertices(i, 0);
+     197       56400 :     points[i].y = vertices(i, 1);
+     198       56400 :     points[i].z = z;
+     199             :   }
+     200             : 
+     201       14100 :   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..93440cebad --- /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-01-07 21:47:14Functions: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&)52
mrs_lib::SafetyZone::~SafetyZone()52
mrs_lib::SafetyZone::isPathValid(double, double, double, double)65
mrs_lib::SafetyZone::isPointValid(double, double)3253
mrs_lib::SafetyZone::getBorder()7050
+
+
+ + + +
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..b598af3049 --- /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-01-07 21:47:14Functions: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)65
mrs_lib::SafetyZone::isPointValid(double, double)3253
mrs_lib::SafetyZone::getBorder()7050
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)52
mrs_lib::SafetyZone::~SafetyZone()52
+
+
+ + + +
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..36d1be7300 --- /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-01-07 21:47:14Functions: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         104 : SafetyZone::~SafetyZone() {
+      17          52 :   delete outerBorder;
+      18          52 : }
+      19             : 
+      20             : //}
+      21             : 
+      22             : /* SafetyZone() //{ */
+      23             : 
+      24          52 : SafetyZone::SafetyZone(const Eigen::MatrixXd& outerBorderMatrix) {
+      25             : 
+      26             :   try {
+      27          52 :     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          52 : }
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* isPointValid() //{ */
+      43             : 
+      44        3253 : bool SafetyZone::isPointValid(const double px, const double py) {
+      45             : 
+      46        3253 :   if (!outerBorder->isPointInside(px, py)) {
+      47          76 :     return false;
+      48             :   }
+      49             : 
+      50        3177 :   return true;
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* isPathValid() //{ */
+      56             : 
+      57          65 : bool SafetyZone::isPathValid(const double p1x, const double p1y, const double p2x, const double p2y) {
+      58             : 
+      59          65 :   if (outerBorder->doesSectionIntersect(p1x, p1y, p2x, p2y)) {
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63          65 :   return true;
+      64             : }
+      65             : 
+      66             : //}
+      67             : 
+      68             : /* getBorder() //{ */
+      69             : 
+      70        7050 : Polygon SafetyZone::getBorder() {
+      71        7050 :   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-01-07 21:47:14Functions: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..ea3267c49d --- /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-01-07 21:47:14Functions: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..c497092861 --- /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-01-07 21:47:14Functions: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..bdd9277572 --- /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-01-07 21:47:14Functions: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..9288062e52 --- /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-01-07 21:47:14Functions: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..2ac9bb33aa --- /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-01-07 21:47:14Functions: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..aa4fbc9a4a --- /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-01-07 21:47:14Functions: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)399
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()399
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2440681
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)3089224
mrs_lib::ScopeTimer::~ScopeTimer()3090762
+
+
+ + + +
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..8860b59ce3 --- /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-01-07 21:47:14Functions: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&)2440681
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)3089224
mrs_lib::ScopeTimer::~ScopeTimer()3090762
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)399
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()399
+
+
+ + + +
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..74916b3f56 --- /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-01-07 21:47:14Functions: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         399 : ScopeTimerLogger::ScopeTimerLogger(const std::string& logfile, const bool enable_logging, const int log_precision)
+      10         399 :     : _logging_enabled_(enable_logging), _log_filepath_(logfile) {
+      11             : 
+      12         399 :   if (!_logging_enabled_) {
+      13         399 :     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         399 : ScopeTimerLogger::~ScopeTimerLogger() {
+      47         399 :   if (_logging_enabled_) {
+      48           0 :     ROS_DEBUG("[%s]: ScopeTimerLogger: closing logstream.", ros::this_node::getName().c_str());
+      49           0 :     _logstream_.close();
+      50             :   }
+      51         399 : }
+      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     3089224 : ScopeTimer::ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable)
+     104     3089224 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(ros::Duration(0.0)), _logger_(scope_timer_logger) {
+     105             : 
+     106     3082786 :   checkpoints.push_back(time_point("timer start"));
+     107             : 
+     108     3081276 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     109     3081276 : }
+     110             : 
+     111             : //}
+     112             : 
+     113             : /* ScopeTimer::checkpoint() //{ */
+     114             : 
+     115     2440681 : void ScopeTimer::checkpoint(const std::string& label) {
+     116             : 
+     117     2440681 :   if (_enable_print_or_log) {
+     118           0 :     checkpoints.push_back(time_point(label));
+     119             :   }
+     120     2440681 : }
+     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     9268511 : ScopeTimer::~ScopeTimer() {
+     136             : 
+     137     3090762 :   if (!_enable_print_or_log) {
+     138     3090821 :     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     3089106 : }
+     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-01-07 21:47:14Functions: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..53897acffb --- /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-01-07 21:47:14Functions: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..1ab3366e8a --- /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-01-07 21:47:14Functions: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..a3a46fb49b --- /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-01-07 21:47:14Functions: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..b6703e55f2 --- /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-01-07 21:47:14Functions: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..318070dd5a --- /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-01-07 21:47:14Functions: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..1f87aa6633 --- /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-01-07 21:47:14Functions: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&)59
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)62
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)62
mrs_lib::TimeoutManager::started(unsigned long)1581
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)7373
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)103085
+
+
+ + + +
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..fde2de7056 --- /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-01-07 21:47:14Functions: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)62
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)7373
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)103085
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)62
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)1581
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&)59
+
+
+ + + +
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..add77847e5 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html @@ -0,0 +1,186 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-01-07 21:47:14Functions: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          59 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7          59 :     : m_last_id(0)
+       8             :   {
+       9          59 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10          59 :   }
+      11             : 
+      12             : 
+      13          62 :   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         124 :     std::scoped_lock lck(m_mtx);
+      16          62 :     const auto new_id = m_timeouts.size();
+      17          62 :     const timeout_info_t new_info = {oneshot, autostart, callback, timeout, last_reset, last_reset};
+      18          62 :     m_timeouts.push_back(new_info);
+      19         124 :     return new_id;
+      20             :   }
+      21             : 
+      22      103085 :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      23             :   {
+      24      103085 :     std::scoped_lock lck(m_mtx);
+      25      103085 :     m_timeouts.at(id).last_reset = time;
+      26      103085 :   }
+      27             : 
+      28           9 :   void TimeoutManager::pause(const timeout_id_t id)
+      29             :   {
+      30           9 :     std::scoped_lock lck(m_mtx);
+      31           9 :     m_timeouts.at(id).started = false;
+      32           9 :   }
+      33             : 
+      34          62 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      35             :   {
+      36          62 :     std::scoped_lock lck(m_mtx);
+      37          62 :     m_timeouts.at(id).started = true;
+      38          62 :     m_timeouts.at(id).last_reset = time;
+      39          62 :   }
+      40             : 
+      41           2 :   void TimeoutManager::pauseAll()
+      42             :   {
+      43           4 :     std::scoped_lock lck(m_mtx);
+      44          10 :     for (auto& timeout_info : m_timeouts)
+      45           8 :       timeout_info.started = false;
+      46           2 :   }
+      47             : 
+      48           4 :   void TimeoutManager::startAll(const ros::Time& time)
+      49             :   {
+      50           8 :     std::scoped_lock lck(m_mtx);
+      51          20 :     for (auto& timeout_info : m_timeouts)
+      52             :     {
+      53          16 :       timeout_info.started = true;
+      54          16 :       timeout_info.last_reset = time;
+      55             :     }
+      56           4 :   }
+      57             : 
+      58           0 :   void TimeoutManager::change(const timeout_id_t id, const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset, const bool oneshot, const bool autostart)
+      59             :   {
+      60           0 :     std::scoped_lock lck(m_mtx);
+      61           0 :     auto& timeout_info = m_timeouts.at(id);
+      62           0 :     timeout_info.oneshot = oneshot;
+      63           0 :     timeout_info.started = autostart;
+      64           0 :     timeout_info.timeout = timeout;
+      65           0 :     timeout_info.callback = callback;
+      66           0 :     timeout_info.last_reset = last_reset;
+      67           0 :   }
+      68             : 
+      69           0 :   ros::Time TimeoutManager::lastReset(const timeout_id_t id)
+      70             :   {
+      71           0 :     std::scoped_lock lck(m_mtx);
+      72           0 :     return m_timeouts.at(id).last_reset;
+      73             :   }
+      74             : 
+      75        1581 :   bool TimeoutManager::started(const timeout_id_t id)
+      76             :   {
+      77        1581 :     std::scoped_lock lck(m_mtx);
+      78        3162 :     return m_timeouts.at(id).started;
+      79             :   }
+      80             : 
+      81        7373 :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      82             :   {
+      83        7373 :     const auto now = ros::Time::now();
+      84             : 
+      85       14746 :     std::scoped_lock lck(m_mtx);
+      86       21028 :     for (auto& timeout_info : m_timeouts)
+      87             :     {
+      88             :       // don't worry, this'll get optimized out by the compiler
+      89       13655 :       const bool started = timeout_info.started;
+      90       13655 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      91       13655 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      92       13655 :       if (started && last_reset_timeout && last_callback_timeout)
+      93             :       {
+      94        1700 :         timeout_info.callback(timeout_info.last_reset);
+      95        1700 :         timeout_info.last_callback = now;
+      96             :         // if the timeout is oneshot, pause it
+      97        1700 :         if (timeout_info.oneshot)
+      98           0 :           timeout_info.started = false;
+      99             :       }
+     100             :     }
+     101        7373 :   }
+     102             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..0b582c4d4a --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html @@ -0,0 +1,46 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
<unnamed>92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-detail-sort-l.html b/mrs_lib/src/timer/index-detail-sort-l.html new file mode 100644 index 0000000000..1e8d18a73f --- /dev/null +++ b/mrs_lib/src/timer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
<unnamed>92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-detail.html b/mrs_lib/src/timer/index-detail.html new file mode 100644 index 0000000000..0f98690f8b --- /dev/null +++ b/mrs_lib/src/timer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
<unnamed>92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-sort-f.html b/mrs_lib/src/timer/index-sort-f.html new file mode 100644 index 0000000000..e244898d80 --- /dev/null +++ b/mrs_lib/src/timer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index-sort-l.html b/mrs_lib/src/timer/index-sort-l.html new file mode 100644 index 0000000000..6c2973fff6 --- /dev/null +++ b/mrs_lib/src/timer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/index.html b/mrs_lib/src/timer/index.html new file mode 100644 index 0000000000..1a89e5253c --- /dev/null +++ b/mrs_lib/src/timer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timer.cpp +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/timer.cpp.func-sort-c.html b/mrs_lib/src/timer/timer.cpp.func-sort-c.html new file mode 100644 index 0000000000..2a0ad9b44e --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func-sort-c.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ThreadTimer::Impl::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::threadFcn()8
mrs_lib::ThreadTimer::Impl::Impl(std::function<void (ros::TimerEvent const&)> const&, ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::~Impl()8
mrs_lib::ThreadTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ROSTimer::setPeriod(ros::Duration const&, bool)8
mrs_lib::ThreadTimer::Impl::stop()24
mrs_lib::ThreadTimer::Impl::start()24
mrs_lib::ThreadTimer::stop()24
mrs_lib::ThreadTimer::start()24
mrs_lib::ROSTimer::stop()24
mrs_lib::ROSTimer::start()24
mrs_lib::ThreadTimer::Impl::breakableSleep(ros::Time const&)208
mrs_lib::ThreadTimer::running()208
mrs_lib::ROSTimer::running()216
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timer/timer.cpp.func.html b/mrs_lib/src/timer/timer.cpp.func.html new file mode 100644 index 0000000000..0e6a8e831b --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer/timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timer - timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-01-07 21:47:14Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

+ Overview +
+ + diff --git a/mrs_lib/src/timer/timer.cpp.gcov.png b/mrs_lib/src/timer/timer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e95d68d4697bd933416509c36f58cb8b9333f03f GIT binary patch literal 1067 zcmV+`1l0S9P)K3`}k|8a=Sbqh~@nKF8rC=ZW zMyHsWD!R5CSI=EeApq(V+p0jYM;YsFJC0*Nj_1~1j~*Zb<+i@Td-Co@t(Si0hwV%x zH^nJ)`?b=*sP{C>AW{6Y&BtvfY^nWD6Lx`LYQjmUi%Y8zngq(NCSAPKgo$GX_^l?K zb3Qzou$4#$3VYQU7hqn7n>qZCmf^_>_i>7t7_65j%s{wW=1z8vj5XHnSz`o6)AN)7 zM_7Xc8Pm@Bu_K)b9ND8i86?+#QpF8P>cp%C6tfN~uf^>OUu-uLV5ER+> z9+}+o@&U4dC{01UFqa^zQoO^~r+Qsw@XjfYH4IgSej=AETwlYGBFM5pKuIZ1bBfzd zID@s^gie^6WorBd_2kl=n$oxC6f_d!fWa9%6b0?)iG#WGCzqA4eT3ug*{v!dzP6RfXRr zz^Khk;sq$|M?phkI7&HA)0Nj2Y`}2S%&X2KR znDvA!dlquHgsW_&FjRDG$MTiF^m2{Y7Yxp@<3h?0?NFH2=XFafCd|q=4?;P+Ma=e lB+m7pa3ozqHtK%8@E@O{r@SB@n9u+K002ovPDHLkV1n4O`B4A> literal 0 HcmV?d00001 diff --git a/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html b/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html new file mode 100644 index 0000000000..cc1889e458 --- /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-01-07 21:47:14Functions: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..f38bf9fa9d --- /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-01-07 21:47:14Functions: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..12f4aec1d9 --- /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-01-07 21:47:14Functions: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..2cde6245c1 --- /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-01-07 21:47:14Functions: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..736bd943d0 --- /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-01-07 21:47:14Functions: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..c0fc7b7af1 --- /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-01-07 21:47:14Functions: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..c15ba3999e --- /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-01-07 21:47:14Functions: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()111
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1247150
+
+
+ + + +
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..be6b0cd219 --- /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-01-07 21:47:14Functions: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&)1247150
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()111
+
+
+ + + +
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..3ef4e1b123 --- /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-01-07 21:47:14Functions: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         111 : TransformBroadcaster::TransformBroadcaster() {
+       8         111 :   broadcaster_ = tf2_ros::TransformBroadcaster();
+       9         111 : }
+      10             : //}
+      11             : 
+      12             : /* sendTransform (const geometry_msgs::TransformStamped &transform) //{ */
+      13     1247150 : void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) {
+      14     2493294 :   std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      15     1245382 :   if (last_messages_.count(frames_from_to) > 0) {
+      16     1245912 :     if (transform.header.stamp > last_messages_[frames_from_to]) {
+      17     1016222 :       broadcaster_.sendTransform(transform);
+      18     1017780 :       last_messages_[frames_from_to] = transform.header.stamp;
+      19             :     } else {
+      20      227820 :       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        1265 :     std::pair<std::string, ros::Time> new_pair;
+      25         620 :     new_pair.first  = frames_from_to;
+      26         620 :     new_pair.second = transform.header.stamp;
+      27         620 :     broadcaster_.sendTransform(transform);
+      28         620 :     last_messages_.insert(new_pair);
+      29             :   }
+      30     1247233 : }
+      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-01-07 21:47:14Functions: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..76134b227b --- /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-01-07 21:47:14Functions: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..38b4ff283b --- /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-01-07 21:47:14Functions: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..2c45dd4996 --- /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-01-07 21:47:14Functions: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..7ad2aae5ad --- /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-01-07 21:47:14Functions: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..4e677ff1d2 --- /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-01-07 21:47:14Functions: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..c7b61f7b89 --- /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-01-07 21:47:14Functions: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&)398
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1023
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1023
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1025
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2056
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&)18768
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)63678
mrs_lib::Transformer::setLatLon(double, double)108940
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&)1122418
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4668043
+
+
+ + + +
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..fa1929f914 --- /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-01-07 21:47:14Functions: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&)18768
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&)63678
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2056
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&)1122418
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4668043
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&)1023
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&)1023
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1025
mrs_lib::Transformer::setLatLon(double, double)108940
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)398
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..d1800c97dd --- /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-01-07 21:47:14Functions: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         398 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49         398 :     : 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         398 :   }
+      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       18768 :   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       37546 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82       18778 :     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       37556 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90       37556 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91       37556 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93       18778 :     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      108940 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119      108940 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122      108497 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123      108598 :     got_utm_zone_ = true;
+     124      108261 :   }
+     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       63678 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255      127356 :     geometry_msgs::PoseStamped pose;
+     256       63678 :     pose.header = what.header;
+     257             :   
+     258       63678 :     pose.pose.position.x = what.reference.position.x;
+     259       63678 :     pose.pose.position.y = what.reference.position.y;
+     260       63678 :     pose.pose.position.z = what.reference.position.z;
+     261       63678 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264      127356 :     const auto pose_opt = transformImpl(tf, pose);
+     265       63678 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268       63678 :     pose = pose_opt.value();
+     269             :   
+     270      127356 :     mrs_msgs::ReferenceStamped ret;
+     271       63678 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274       63678 :     ret.reference.position.x = pose.pose.position.x;
+     275       63678 :     ret.reference.position.y = pose.pose.position.y;
+     276       63678 :     ret.reference.position.z = pose.pose.position.z;
+     277       63678 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278       63678 :     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     1122418 :   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     1122418 :     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     1122418 :     if (from_frame.empty() || to_frame.empty())
+     312             :     {
+     313           3 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform to/from an empty frame!", node_name_.c_str());
+     314           3 :       return std::nullopt;
+     315             :     }
+     316             : 
+     317             :     // if the frames are the same, just return an identity transform
+     318     1122406 :     if (from_frame == to_frame)
+     319      216328 :       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     1014246 :     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     1014247 :     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        2052 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337        2052 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338        1026 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341        1026 :       frame_to(*tf_opt) = to_frame;
+     342        1026 :       return tf_opt;
+     343             :     }
+     344             : 
+     345     3039524 :     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     1494823 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352     1062815 :     catch (tf2::TransformException& e)
+     353             :     {
+     354      531411 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358      531408 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362     1054576 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364       16312 :       catch (tf2::TransformException& e)
+     365             :       {
+     366        8156 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371        8186 :     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        8186 :       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        8186 :     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     4668043 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466     4668043 :     if (frame_id.empty())
+     467       46366 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470     4621660 :     if (prefix_.empty())
+     471     2963139 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474     1658410 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475     1095000 :       return prefix_ + frame_id;
+     476             : 
+     477      563596 :     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        1025 :   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        1025 :     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        1024 :     geometry_msgs::Point latlon;
+     532        1024 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533        1024 :     latlon.z = what.z;
+     534        1024 :     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        1023 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552        1023 :     const auto opt = UTMtoLL(what.position, prefix);
+     553        1023 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556        1023 :     geometry_msgs::Pose ret;
+     557        1023 :     ret.position = opt.value();
+     558        1023 :     ret.orientation = what.orientation;
+     559        1023 :     return ret;
+     560             :   }
+     561             : 
+     562        1023 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564        1023 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565        1023 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568        2046 :     geometry_msgs::PoseStamped ret;
+     569        1023 :     ret.header.frame_id = prefix + "utm_origin";
+     570        1023 :     ret.header.stamp = what.header.stamp;
+     571        1023 :     ret.pose = opt.value();
+     572        1023 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577        2056 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579        2056 :     const auto firstof = frame_id.find_first_of('/');
+     580        2056 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583        2056 :       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..951c26465a --- /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-01-07 21:47:14Functions: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..307af3b76a --- /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-01-07 21:47:14Functions: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..a2d67285c0 --- /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-01-07 21:47:14Functions: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..c501fb2421 --- /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-01-07 21:47:14Functions: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..e09ac33e83 --- /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-01-07 21:47:14Functions: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..a168b167c7 --- /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-01-07 21:47:14Functions: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..221e1a47d5 --- /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-01-07 21:47:14Functions: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>&)296661
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()296665
+
+
+ + + +
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..9293226238 --- /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-01-07 21:47:14Functions: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>&)296661
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()296665
+
+
+ + + +
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..155a9bac7f --- /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-01-07 21:47:14Functions: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      296661 : AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>& in)
+       7      296661 :   : variable(in)
+       8             : {
+       9      296661 :   variable = true;
+      10      296660 : }
+      11             : 
+      12      593330 : AtomicScopeFlag::~AtomicScopeFlag()
+      13             : {
+      14      296665 :   variable = false;
+      15      296665 : }
+      16             : 
+      17             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/utils/utils.cpp.gcov.overview.html b/mrs_lib/src/utils/utils.cpp.gcov.overview.html new file mode 100644 index 0000000000..f3b2e0a717 --- /dev/null +++ b/mrs_lib/src/utils/utils.cpp.gcov.overview.html @@ -0,0 +1,25 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/utils/utils.cpp + + + + + + + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()3
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()9
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)10
(anonymous namespace)::ProxyExec0::ProxyExec0()13
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()13
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)22
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)27
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()77
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)707
mrs_uav_autostart::automatic_start::AutomaticStart::speedCheck()2856
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()2856
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()2856
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()2856
mrs_uav_autostart::automatic_start::Topic::getTime()5789
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)6386
mrs_uav_autostart::automatic_start::Topic::updateTime()16110
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&) const16112
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)16113
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)41626
+
+
+ + + +
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..b5e9efdcb6 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()13
mrs_uav_autostart::automatic_start::AutomaticStart::speedCheck()2856
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()2856
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)22
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)16113
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()2856
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()2856
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)41626
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)10
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)707
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()3
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()13
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()9
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)6386
mrs_uav_autostart::automatic_start::Topic::updateTime()16110
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()77
mrs_uav_autostart::automatic_start::Topic::getTime()5789
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)27
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&) const16112
+
+
+ + + +
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..a1d66cc591 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,887 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.png b/mrs_uav_autostart/src/automatic_start.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1ad4a95611cf51a0144444214619a688390d984b GIT binary patch literal 2658 zcmV-o3Z3fB;{Y$NG3Yw(pNc z3%=<&qK_WQj3_`JT-w7XLL8yUqKgs5Am(w7BCnhQ#L1-_q)9}I0|Jp|lv%ouD6<|F zEfEy2kUJ@A>>8$c3Mr9RePy(Clll!yuMhvZYSDJ0YyxU>D|i3}brHAay1hbzGD7@0S9p~2;kjS+G&C5He5es2=IFj%&dU9ki2DzE6Cwi@D*hnrwFMP zuyqjmY`CJx<<})5NwUG?u)KjH)MFqExwxQ53YKL}-|;vO`iO-t6qfd|WyC9b+#pFE zJzjIeC6Aj9j}nLNI9}7rS(@_K&_?N8hk?}V7Twh~sWY?n)})_nj~gNrMezY$q=M*K z>JSUD+*hQIK>0~{xHbkc#hvQcG2#Z6e&E$FR|J4jtnLG)BeO?AaxL_F$VeiFp_$Z~h zB8R7#gxR1ZO7XgRa3TB63rM|vSIrXZui>VVvSpov-qPv97!rUjys}p6S?QJ2b~*q5 zG37Ab4rDIlwyUIrRaaNjM1^ax$SlQ9m3xrl_usZ(@I@7PzOp)6l-~EVAl@#V$BJ;L zx<92yQ+b-HUFowe%&51ob{*z*Jj0=}8SSDQeDBr1glizC53A z1YhUp*ZcMT;o<24AU>P0JYQ{ugeV8~%5bIdQ(Np_C%xH5%QsSrh(YIV>#r2uS?O^QmC>;T?Bi>6^-74cIaw{=G+0cLaz+iW59)&Bc-t zBkm(GK4NE_KQX1}B{KkW3qQ;#lr+vLynNA2-89?Nv+e>ZxEtPZfh4bc##YW^!uMd! z5r|TXahh}Moy|mR`#PH7aO8Y;a_oT*IvyT+CH zJTv)bmX2@e;Z-}MbzJuyHeULONAHn^ThC0~9_HAjGvohiwKQa~Oq`kSb05N`pT0=? zdUdL#--xmpcf*+x3XQJzO*@nKkZw5te<2UQ+N0)k+@qJ{ati<0lq)Ap<0WLV`Rq(` zR;CTpRW9}lJ57;2V*mQ#l3jAEIidS6xr$GIG7bRVa9vt*g9C78Ca|D;<^ZzTwyv%N zVT!q(N>dc#UQqM_&K+OJxk_S@kBFdx&6ivQL!f!^qQcG2}uVUAX9^qa$)lL7QgMB(8+FaNDalqBpY=%!~;7Gyq0 zy#d1t&s37ieb|hhz>L_5U=>BeNn}JxIMi1o>sNLT(}-WthblJa@Qw4yTR+ z!5)0MeH7x)U+=Gs*f|bkXGRR$rKj~vkO>_ffZ)=;2!(c1-%@wod}Sn+_Xw;VMg_~~ zbHhg=Qi6kaC~+tenWSrME`|1w;E>zw&7>41H@QHOJo5x1KaSF*$W?}SktZE9AKk+h zWdV`h7Mn7$*Jb1~q2&fG;HTqE=aG*njCOG!ucMYcfxZ&zuDmXPFCtV=6pT5#ok9z#yQtNZF$Xdbg zsWn#)=r0UZ=@px~W|JvAq{ft;3x}tZa6iq|_OvQ-XUgnT>aTv*EP>F;6(LGHxl+)N zt~&~vmQ4_ydeGBAFMl~Xp9eY>{Ca~r#hZdc87HDhKO^b^MI4n;j$Ks5$?E9}beGis zR?rk7-Y}OUe20V^1~1a_Yf3MlOcldj(N(cYT1GC`(i9?B(FU2I$Ug`;PO&UVlYVWm zr`an2j8Q~aE|vFHh{YK-9@Q^Mah40$>E0m>hI)M{*=c70>ar}nEa_R_Gs?ej7zBC- zopmV+R~}gUQwQ>?(>v$((xX<26hRspV2UE;_DX`k$nEitPo-GkXG6eqalhU_Vqq*2 zE7to>c#`C(VlFL=dbam>9cgOke!V@S!0*XD?xzj-={QLKfhD6|(1>5IpfNU%m#N4Q zJC9XpInp`x{LddX;MsB&A%FBK*T0cJhGh7E{@A4W5pQ}2C^j6ySa}xNOfsVwH@{wa z{7JHf`q5Pk@}by4h4<@^x(a`Pj>749eN=D2AXfPAir3Wpat%=YAFnmw*@$uek8~9N zjd=a~AFulq;h?)p)MiFFuNHg-0ecZ1!u`HC1?{yyh*Hz_qD#Ef*|vVFde4Do`wT=Z zOKfQ{49$ID#K!yHjQGbNoB5CxaXzYjrJ0G>=b8?jgQ-H>aKbjIsmH#fWEx>rTvHjh771Bku?!sO0`|Hh+#3)s20BQ)3v4jM(_ zK*_g?=DJsL7%)5tZ5Cm}=dMIkkm7!ZOVJ;*Ehs9GjF107AQ5S+t&I5i0}*nYn#T)3 Q5&!@I07*qoM6N<$f>>%4761SM literal 0 HcmV?d00001 diff --git a/mrs_uav_autostart/src/index-detail-sort-f.html b/mrs_uav_autostart/src/index-detail-sort-f.html new file mode 100644 index 0000000000..6f517c10b5 --- /dev/null +++ b/mrs_uav_autostart/src/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
<unnamed>88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail-sort-l.html b/mrs_uav_autostart/src/index-detail-sort-l.html new file mode 100644 index 0000000000..d4c925cfeb --- /dev/null +++ b/mrs_uav_autostart/src/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
<unnamed>88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail.html b/mrs_uav_autostart/src/index-detail.html new file mode 100644 index 0000000000..a118d22092 --- /dev/null +++ b/mrs_uav_autostart/src/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
<unnamed>88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-f.html b/mrs_uav_autostart/src/index-sort-f.html new file mode 100644 index 0000000000..6c60480a78 --- /dev/null +++ b/mrs_uav_autostart/src/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-l.html b/mrs_uav_autostart/src/index-sort-l.html new file mode 100644 index 0000000000..ec5cc9577d --- /dev/null +++ b/mrs_uav_autostart/src/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index.html b/mrs_uav_autostart/src/index.html new file mode 100644 index 0000000000..7f25780df4 --- /dev/null +++ b/mrs_uav_autostart/src/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23626888.1 %
Date:2024-01-07 21:47:14Functions:1919100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
88.1%88.1%
+ 		88.1 %236 / 268100.0 %19 / 19
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.func-sort-c.html b/mrs_uav_controllers/include/common.h.func-sort-c.html new file mode 100644 index 0000000000..cabc9a4718 --- /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-01-07 21:47:14Functions: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::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)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::HwApiAttitudeRateCmd_<std::allocator<void> > const&)21
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)25
+
+
+ + + +
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..1a980c80d2 --- /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-01-07 21:47:14Functions: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&)25
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)21
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&)2
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..037558bf1c --- /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-01-07 21:47:14Functions: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          25 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      77          25 :     return msg.throttle;
+      78             :   }
+      79          21 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      80          21 :     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           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      89           2 :     return std::nullopt;
+      90             :   }
+      91           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      92           2 :     return std::nullopt;
+      93             :   }
+      94           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+      95           2 :     return std::nullopt;
+      96             :   }
+      97             : };
+      98             : 
+      99             : //}
+     100             : 
+     101             : }  // namespace common
+     102             : 
+     103             : }  // namespace mrs_uav_controllers
+     104             : 
+     105             : #endif  // MRS_UAV_CONTROLLERS_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.gcov.overview.html b/mrs_uav_controllers/include/common.h.gcov.overview.html new file mode 100644 index 0000000000..42538a882f --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::reset()684
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)684
mrs_uav_controllers::PIDController::PIDController()684
mrs_uav_controllers::PIDController::setSaturation(double)4199
mrs_uav_controllers::PIDController::update(double const&, double const&)4199
+
+
+ + + +
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..e02ff5287e --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:313393.9 %
Date:2024-01-07 21:47:14Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_controllers/include/pid.hpp.gcov.png b/mrs_uav_controllers/include/pid.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9667ce26842a9ead24557971d5cb3a0df01cd47f GIT binary patch literal 533 zcmV+w0_y#VP)#@dQ9wSy;%iQAIeL4O+=LtvN-1eSVrXn2+bp93 zwUSeXAft77SEfO(qhq;>+mSJm0O~TxRd^JUBjE-K)`1eit@FzQHgN^8jpLgF8FT?J zi@U-d8D+{`dnoiJBcK=o`7u)6)`r;8IG5B(Rp%pPM zol@s!@vAf2+{Z$pi7}lM6_Xb*`4`;BshnnH+h1;aYnW&~xC0~A5QuC>F3`rGr-Lpf z_nj|hld&tUy9vDpbm8i*o_ydx2u?~Dy)@rFzfT5R9v&ov$g4GO1K=B37d6zOhS#RS zNya#(AQ`sx$+VGg#H-+TT_zdjaksHMSmr9Dsn0O7YCIL77%D*fECLf;9k^A2b1~|Z zH2J|C$Y&&$W^)sxpO&|Md;=O|j(tCz?WSYGmrlp4!|FLm?-W*j4X?6FPKndJGaQ<=0 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13515984.9 %
Date:2024-01-07 21:47:14Functions: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&)136
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3734
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&)7489
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)9911
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&)48928
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&)48928
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)50945
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&)50945
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)52309
+
+
+ + + +
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..cf3daf5db7 --- /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:13515984.9 %
Date:2024-01-07 21:47:14Functions: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&)48928
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3734
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)52309
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)136
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)9911
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)50945
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&)50945
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&)48928
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&)7489
+
+
+ + + +
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..1c4248fb60 --- /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:13515984.9 %
Date:2024-01-07 21:47:14Functions: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       50945 : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd) {
+      12             : 
+      13             :   // orientation error
+      14       50945 :   Eigen::Matrix3d R_error = 0.5 * (Rd.transpose() * R - R.transpose() * Rd);
+      15             : 
+      16             :   // vectorize the orientation error
+      17             :   // clang-format off
+      18       50945 :     Eigen::Vector3d R_error_vec;
+      19       50945 :     R_error_vec << (R_error(1, 2) - R_error(2, 1)) / 2.0,
+      20       50945 :                    (R_error(2, 0) - R_error(0, 2)) / 2.0,
+      21       50945 :                    (R_error(0, 1) - R_error(1, 0)) / 2.0;
+      22             :   // clang-format on
+      23             : 
+      24      101890 :   return R_error_vec;
+      25             : }
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* sanitizeDesiredForce() //{ */
+      30             : 
+      31       48928 : 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       48928 :   double theta = acos(input[2]);
+      36       48928 :   double phi   = atan2(input[1], input[0]);
+      37             : 
+      38             :   // check for the failsafe limit
+      39       48928 :   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       48928 :   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       48928 :   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       48928 :   Eigen::Vector3d output(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
+      63             : 
+      64       48928 :   return {output};
+      65             : }
+      66             : 
+      67             : //}
+      68             : 
+      69             : /* so3transform() //{ */
+      70             : 
+      71       48928 : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
+      72             : 
+      73       48928 :   Eigen::Vector3d body_z_normed = body_z.normalized();
+      74             : 
+      75       48928 :   Eigen::Matrix3d Rd;
+      76             : 
+      77       48928 :   if (preserve_heading) {
+      78             : 
+      79       17327 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Baca's method");
+      80             : 
+      81             :     // | ------------------------- body z ------------------------- |
+      82       17327 :     Rd.col(2) = body_z_normed;
+      83             : 
+      84             :     // | ------------------------- body x ------------------------- |
+      85             : 
+      86             :     // construct the oblique projection
+      87       17327 :     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       34654 :     Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+      91       17327 :     A.col(0)          = projector_body_z_compl.col(0);
+      92       17327 :     A.col(1)          = projector_body_z_compl.col(1);
+      93             : 
+      94             :     // create the basis of the projection null-space complement
+      95       34654 :     Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+      96       17327 :     B.col(0)          = Eigen::Vector3d(1, 0, 0);
+      97       17327 :     B.col(1)          = Eigen::Vector3d(0, 1, 0);
+      98             : 
+      99             :     // oblique projector to <range_basis>
+     100       34654 :     Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     101       34654 :     Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     102       17327 :     Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     103             : 
+     104       17327 :     Rd.col(0) = oblique_projector * heading;
+     105       17327 :     Rd.col(0).normalize();
+     106             : 
+     107             :     // | ------------------------- body y ------------------------- |
+     108             : 
+     109       17327 :     Rd.col(1) = Rd.col(2).cross(Rd.col(0));
+     110       17327 :     Rd.col(1).normalize();
+     111             : 
+     112             :   } else {
+     113             : 
+     114       31601 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Lee's method");
+     115             : 
+     116       31601 :     Rd.col(2) = body_z_normed;
+     117       31601 :     Rd.col(1) = Rd.col(2).cross(heading);
+     118       31601 :     Rd.col(1).normalize();
+     119       31601 :     Rd.col(0) = Rd.col(1).cross(Rd.col(2));
+     120       31601 :     Rd.col(0).normalize();
+     121             :   }
+     122             : 
+     123       97856 :   return Rd;
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* getLowestOutput() //{ */
+     129             : 
+     130       52309 : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     131             : 
+     132       52309 :   if (outputs.actuators) {
+     133        2341 :     return ACTUATORS_CMD;
+     134             :   }
+     135             : 
+     136       49968 :   if (outputs.control_group) {
+     137        2362 :     return CONTROL_GROUP;
+     138             :   }
+     139             : 
+     140       47606 :   if (outputs.attitude_rate) {
+     141       42063 :     return ATTITUDE_RATE;
+     142             :   }
+     143             : 
+     144        5543 :   if (outputs.attitude) {
+     145        2162 :     return ATTITUDE;
+     146             :   }
+     147             : 
+     148        3381 :   if (outputs.acceleration_hdg_rate) {
+     149         955 :     return ACCELERATION_HDG_RATE;
+     150             :   }
+     151             : 
+     152        2426 :   if (outputs.acceleration_hdg) {
+     153        1020 :     return ACCELERATION_HDG;
+     154             :   }
+     155             : 
+     156        1406 :   if (outputs.velocity_hdg_rate) {
+     157         481 :     return VELOCITY_HDG_RATE;
+     158             :   }
+     159             : 
+     160         925 :   if (outputs.velocity_hdg) {
+     161         440 :     return VELOCITY_HDG;
+     162             :   }
+     163             : 
+     164         485 :   if (outputs.position) {
+     165         485 :     return POSITION;
+     166             :   }
+     167             : 
+     168           0 :   return {};
+     169             : }
+     170             : 
+     171             : //}
+     172             : 
+     173             : /* getHighestOutput() //{ */
+     174             : 
+     175        9911 : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     176             : 
+     177        9911 :   if (outputs.position) {
+     178           3 :     return POSITION;
+     179             :   }
+     180             : 
+     181        9908 :   if (outputs.velocity_hdg) {
+     182           2 :     return VELOCITY_HDG;
+     183             :   }
+     184             : 
+     185        9906 :   if (outputs.velocity_hdg_rate) {
+     186           2 :     return VELOCITY_HDG_RATE;
+     187             :   }
+     188             : 
+     189        9904 :   if (outputs.acceleration_hdg) {
+     190           5 :     return ACCELERATION_HDG;
+     191             :   }
+     192             : 
+     193        9899 :   if (outputs.acceleration_hdg_rate) {
+     194           5 :     return ACCELERATION_HDG_RATE;
+     195             :   }
+     196             : 
+     197        9894 :   if (outputs.attitude) {
+     198        5663 :     return ATTITUDE;
+     199             :   }
+     200             : 
+     201        4231 :   if (outputs.attitude_rate) {
+     202        1408 :     return ATTITUDE_RATE;
+     203             :   }
+     204             : 
+     205        2823 :   if (outputs.control_group) {
+     206        1412 :     return CONTROL_GROUP;
+     207             :   }
+     208             : 
+     209        1411 :   if (outputs.actuators) {
+     210        1411 :     return ACTUATORS_CMD;
+     211             :   }
+     212             : 
+     213           0 :   return {};
+     214             : }
+     215             : 
+     216             : //}
+     217             : 
+     218             : /* extractThrottle() //{ */
+     219             : 
+     220         136 : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output) {
+     221             : 
+     222         136 :   if (!control_output.control_output) {
+     223          74 :     return {};
+     224             :   }
+     225             : 
+     226         124 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* attitudeController() //{ */
+     232             : 
+     233       50945 : 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       50945 :   Eigen::Matrix3d R  = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     238       50945 :   Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
+     239             : 
+     240             :   // calculate the orientation error
+     241       50945 :   Eigen::Vector3d E = common::orientationError(R, Rd);
+     242             : 
+     243       50945 :   Eigen::Vector3d rate_feedback = gains.array() * E.array() + ff_rate.array();
+     244             : 
+     245             :   // | ----------- parasitic heading rate compensation ---------- |
+     246             : 
+     247       50945 :   if (parasitic_heading_rate_compensation) {
+     248             : 
+     249       16210 :     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       16210 :     Eigen::Vector3d rp_heading_rate_compensation(0, 0, 0);
+     253             : 
+     254       16210 :     Eigen::Vector3d q_feedback_yawless = rate_feedback;
+     255       16210 :     q_feedback_yawless(2)              = 0;  // nullyfy the effect of the original yaw feedback
+     256             : 
+     257       16210 :     double parasitic_heading_rate = 0;
+     258             : 
+     259             :     try {
+     260       16210 :       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       16210 :       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       16210 :     rate_feedback += rp_heading_rate_compensation;
+     274             :   }
+     275             : 
+     276             :   // | --------------- saturate the attitude rate --------------- |
+     277             : 
+     278       50945 :   if (rate_feedback[0] > rate_saturation[0]) {
+     279           0 :     rate_feedback[0] = rate_saturation[0];
+     280       50945 :   } else if (rate_feedback[0] < -rate_saturation[0]) {
+     281           0 :     rate_feedback[0] = -rate_saturation[0];
+     282             :   }
+     283             : 
+     284       50945 :   if (rate_feedback[1] > rate_saturation[1]) {
+     285           0 :     rate_feedback[1] = rate_saturation[1];
+     286       50945 :   } else if (rate_feedback[1] < -rate_saturation[1]) {
+     287           0 :     rate_feedback[1] = -rate_saturation[1];
+     288             :   }
+     289             : 
+     290       50945 :   if (rate_feedback[2] > rate_saturation[2]) {
+     291           0 :     rate_feedback[2] = rate_saturation[2];
+     292       50945 :   } 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       50945 :   mrs_msgs::HwApiAttitudeRateCmd cmd;
+     299             : 
+     300       50945 :   cmd.stamp = ros::Time::now();
+     301             : 
+     302       50945 :   cmd.body_rate.x = rate_feedback[0];
+     303       50945 :   cmd.body_rate.y = rate_feedback[1];
+     304       50945 :   cmd.body_rate.z = rate_feedback[2];
+     305             : 
+     306       50945 :   cmd.throttle = reference.throttle;
+     307             : 
+     308      101890 :   return cmd;
+     309             : }
+     310             : 
+     311             : //}
+     312             : 
+     313             : /* attitudeRateController() //{ */
+     314             : 
+     315        7489 : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+     316             :                                                                      const Eigen::Vector3d& gains) {
+     317             : 
+     318        7489 :   Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
+     319        7489 :   Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     320             : 
+     321        7489 :   Eigen::Vector3d ctrl_group_action = (des_rate - cur_rate).array() * gains.array();
+     322             : 
+     323        7489 :   mrs_msgs::HwApiControlGroupCmd cmd;
+     324             : 
+     325        7489 :   cmd.stamp = ros::Time::now();
+     326             : 
+     327        7489 :   cmd.throttle = reference.throttle;
+     328             : 
+     329        7489 :   cmd.roll  = ctrl_group_action[0];
+     330        7489 :   cmd.pitch = ctrl_group_action[1];
+     331        7489 :   cmd.yaw   = ctrl_group_action[2];
+     332             : 
+     333       14978 :   return {cmd};
+     334             : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* actuatorMixer() //{ */
+     339             : 
+     340        3734 : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
+     341             : 
+     342        3734 :   Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
+     343             : 
+     344        7468 :   Eigen::VectorXd motors = mixer * ctrl_group;
+     345             : 
+     346        3734 :   double min = motors.minCoeff();
+     347             : 
+     348        3734 :   if (min < 0.0) {
+     349           1 :     motors.array() += abs(min);
+     350             :   }
+     351             : 
+     352        3734 :   double max = motors.maxCoeff();
+     353             : 
+     354        3734 :   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        3734 :   mrs_msgs::HwApiActuatorCmd actuator_msg;
+     373             : 
+     374        3734 :   actuator_msg.stamp = ros::Time::now();
+     375             : 
+     376       18670 :   for (int i = 0; i < motors.size(); i++) {
+     377       14936 :     actuator_msg.motors.push_back(motors[i]);
+     378             :   }
+     379             : 
+     380        7468 :   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..1e547626ee0285bef0d938ffe6a6384aa699d3a2 GIT binary patch literal 1366 zcmV-c1*!UpP)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}jedXNrVphT@^;f; zS=M#C4#E_k8|x$<{Yz_EfA2$7yvn=qxxbgfQB;^mcnT3&udVwO$XYtTzT!qFDUn7z zI84@+->mHUZRazkLW;!qVrFug5dhNiEL8U@hGKDsNdwpJRJ1AHVHh^)u3&$P=&5zQ Yf1F4(w@zod)&Kwi07*qoM6N<$g0IhoX8-^I literal 0 HcmV?d00001 diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html b/mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html new file mode 100644 index 0000000000..34ad3de84e --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-01-07 21:47:14Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()27
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)227
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()971
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9713
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)64638
+
+
+ + + +
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..567db74ea4 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-01-07 21:47:14Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()27
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>)57
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9713
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)227
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)64638
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()971
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html new file mode 100644 index 0000000000..482a0a3bc5 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html new file mode 100644 index 0000000000..732a5cd419 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html @@ -0,0 +1,635 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-01-07 21:47:14Functions: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          57 : 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          57 :   nh_ = nh;
+     117             : 
+     118          57 :   common_handlers_  = common_handlers;
+     119          57 :   private_handlers_ = private_handlers;
+     120             : 
+     121          57 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123          57 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127         114 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129          57 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131          57 :   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          57 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139          57 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141         114 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143          57 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144          57 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146          57 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148          57 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150          57 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151          57 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153          57 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158          57 :   _descend_speed_        = std::abs(_descend_speed_);
+     159          57 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161          57 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165          57 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166          57 :   shopts.nh                 = nh_;
+     167          57 :   shopts.node_name          = "FailsafeController";
+     168          57 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169          57 :   shopts.threadsafe         = true;
+     170          57 :   shopts.autostart          = true;
+     171          57 :   shopts.queue_size         = 10;
+     172          57 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174          57 :   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          57 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182          57 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186          57 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188          57 :   is_initialized_ = true;
+     189             : 
+     190          57 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           7 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199          14 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           7 :   if (!last_control_output.control_output) {
+     202             : 
+     203           0 :     ROS_WARN("[FailsafeController]: activated without getting the last controller's command");
+     204             : 
+     205           0 :     return false;
+     206             : 
+     207             :   } else {
+     208             : 
+     209             :     // | -------------- calculate the initial heading ------------- |
+     210             : 
+     211           7 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213          14 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           7 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           7 :       ROS_INFO("[FailsafeController]: activated with heading = %.2f rad", heading_setpoint_);
+     218             : 
+     219             :     } else {
+     220             : 
+     221           0 :       ROS_ERROR("[FailsafeController]: missing orientation from HW API, activated with heading = 0 rad");
+     222             : 
+     223           0 :       heading_setpoint_ = 0;
+     224             :     }
+     225             : 
+     226           7 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           7 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           7 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           0 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           7 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           7 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           7 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           7 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           7 :   first_iteration_ = true;
+     243             : 
+     244           7 :   is_active_ = true;
+     245             : 
+     246           7 :   return true;
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* deactivate() //{ */
+     252             : 
+     253          27 : void FailsafeController::deactivate(void) {
+     254             : 
+     255          27 :   is_active_           = false;
+     256          27 :   first_iteration_     = false;
+     257          27 :   uav_mass_difference_ = 0;
+     258             : 
+     259          27 :   ROS_INFO("[FailsafeController]: deactivated");
+     260          27 : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* updateInactive() //{ */
+     265             : 
+     266       64638 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268       64638 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270       64638 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272       64638 :   first_iteration_ = false;
+     273       64638 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279        9713 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282       29139 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283       29139 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286       19426 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288        9713 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291        9713 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295        9713 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301        9713 :   if (first_iteration_) {
+     302           7 :     dt               = 0.01;
+     303           7 :     first_iteration_ = false;
+     304             :   } else {
+     305        9706 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308        9713 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310        9713 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312        9713 :   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        9713 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317        9713 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323       19426 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325        9713 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327        9713 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329        9713 :   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        9713 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340        9713 :   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        9713 :   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        9713 :   if (highest_modality.value() == common::VELOCITY_HDG_RATE) {
+     366             : 
+     367           0 :     mrs_msgs::HwApiVelocityHdgRateCmd vel_cmd;
+     368             : 
+     369           0 :     vel_cmd.header.stamp = ros::Time::now();
+     370             : 
+     371           0 :     vel_cmd.velocity.x   = 0;
+     372           0 :     vel_cmd.velocity.y   = 0;
+     373           0 :     vel_cmd.velocity.z   = -_descend_speed_;
+     374           0 :     vel_cmd.heading_rate = 0.0;
+     375             : 
+     376           0 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning velocity+hdg rate output");
+     377           0 :     control_output.control_output = vel_cmd;
+     378           0 :     return control_output;
+     379             :   }
+     380             : 
+     381             :   // --------------------------------------------------------------
+     382             :   // |                     acceleration output                    |
+     383             :   // --------------------------------------------------------------
+     384             : 
+     385        9713 :   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        9713 :   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        9713 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423        9713 :   attitude_cmd.stamp       = ros::Time::now();
+     424        9713 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425        9713 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427        9713 :   if (highest_modality.value() == common::ATTITUDE) {
+     428        5534 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude output");
+     429        5534 :     control_output.control_output = attitude_cmd;
+     430        5534 :     return control_output;
+     431             :   }
+     432             : 
+     433             :   // --------------------------------------------------------------
+     434             :   // |                      attitude control                      |
+     435             :   // --------------------------------------------------------------
+     436             : 
+     437        4179 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438        4179 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439        4179 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441        4179 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443        4179 :   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        2786 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455        2786 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457        2786 :   if (highest_modality.value() == common::CONTROL_GROUP) {
+     458        1393 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning control group output");
+     459        1393 :     control_output.control_output = control_group_command;
+     460        1393 :     return control_output;
+     461             :   }
+     462             : 
+     463             :   // --------------------------------------------------------------
+     464             :   // |                            mixer                           |
+     465             :   // --------------------------------------------------------------
+     466             : 
+     467        2786 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469        1393 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470        1393 :   control_output.control_output = actuator_cmd;
+     471        1393 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478         971 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480         971 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482         971 :   controller_status.active = is_active_;
+     483             : 
+     484         971 :   return controller_status;
+     485             : }
+     486             : 
+     487             : //}
+     488             : 
+     489             : /* switchOdometrySource() //{ */
+     490             : 
+     491           0 : void FailsafeController::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     492           0 : }
+     493             : 
+     494             : //}
+     495             : 
+     496             : /* resetDisturbanceEstimators() //{ */
+     497             : 
+     498           0 : void FailsafeController::resetDisturbanceEstimators(void) {
+     499           0 : }
+     500             : 
+     501             : //}
+     502             : 
+     503             : /* setConstraints() //{ */
+     504             : 
+     505         227 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         227 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         227 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         227 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         454 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         227 :   res.success = true;
+     518         227 :   res.message = "constraints updated";
+     519             : 
+     520         227 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           7 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           7 :     return mrs_lib::AttitudeConverter(quaternion).getHeading();
+     531             :   }
+     532           0 :   catch (...) {
+     533             :   }
+     534             : 
+     535             :   try {
+     536           0 :     return mrs_lib::AttitudeConverter(quaternion).getYaw();
+     537             :   }
+     538           0 :   catch (...) {
+     539             :   }
+     540             : 
+     541           0 :   return 0;
+     542             : }
+     543             : 
+     544             : //}
+     545             : 
+     546             : }  // namespace failsafe_controller
+     547             : 
+     548             : }  // namespace mrs_uav_controllers
+     549             : 
+     550             : #include <pluginlib/class_list_macros.h>
+     551          57 : PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::failsafe_controller::FailsafeController, mrs_uav_managers::Controller)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html new file mode 100644 index 0000000000..ba53a24ec7 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html @@ -0,0 +1,158 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
<unnamed>67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
78.9%78.9%
+ 		78.9 %611 / 77488.2 %15 / 17
<unnamed>78.9 %611 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
<unnamed>82.4 %737 / 89488.9 %16 / 18
common.cpp +
84.9%84.9%
+ 		84.9 %135 / 159100.0 %9 / 9
<unnamed>84.9 %135 / 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..94a824ad41 --- /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:1748217280.5 %
Date:2024-01-07 21:47:14Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
<unnamed>67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
78.9%78.9%
+ 		78.9 %611 / 77488.2 %15 / 17
<unnamed>78.9 %611 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
<unnamed>82.4 %737 / 89488.9 %16 / 18
common.cpp +
84.9%84.9%
+ 		84.9 %135 / 159100.0 %9 / 9
<unnamed>84.9 %135 / 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..041b266749 --- /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:1748217280.5 %
Date:2024-01-07 21:47:14Functions: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.9%84.9%
+ 		84.9 %135 / 159100.0 %9 / 9
<unnamed>84.9 %135 / 159100.0 %9 / 9
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
<unnamed>67.5 %131 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
<unnamed>82.4 %737 / 89488.9 %16 / 18
se3_controller.cpp +
78.9%78.9%
+ 		78.9 %611 / 77488.2 %15 / 17
<unnamed>78.9 %611 / 77488.2 %15 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/index-sort-f.html b/mrs_uav_controllers/src/index-sort-f.html new file mode 100644 index 0000000000..1a19d9acb7 --- /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:1748217280.5 %
Date:2024-01-07 21:47:14Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
78.9%78.9%
+ 		78.9 %611 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
common.cpp +
84.9%84.9%
+ 		84.9 %135 / 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..8dabcf43d2 --- /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:1748217280.5 %
Date:2024-01-07 21:47:14Functions:586687.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
78.9%78.9%
+ 		78.9 %611 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
common.cpp +
84.9%84.9%
+ 		84.9 %135 / 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..9055047ea6 --- /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:1748217280.5 %
Date:2024-01-07 21:47:14Functions: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.9%84.9%
+ 		84.9 %135 / 159100.0 %9 / 9
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
se3_controller.cpp +
78.9%78.9%
+ 		78.9 %611 / 77488.2 %15 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html new file mode 100644 index 0000000000..1d9aa1bfc4 --- /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-01-07 21:47:14Functions: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&)10
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()57
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>)57
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()70
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)88
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)198
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)227
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)74153
+
+
+ + + +
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..24e699335c --- /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-01-07 21:47:14Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::mpc_controller::MpcController::callbackSetIntegralTerms(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_controllers::mpc_controller::MpcController::resetDisturbanceEstimators()0
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)4
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_controllers::mpc_controller::MpcController::deactivate()101
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>)114
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)114
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)120
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)259
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)454
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&)463
mrs_uav_controllers::mpc_controller::MpcController::getStatus()6554
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)7536
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&)32563
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)33026
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)33285
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)60288
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)115417
+
+
+ + + +
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..1072a49141 --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/mpc_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - mpc_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:73789482.4 %
Date:2024-01-07 21:47:14Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/mpc_controller.cpp.gcov.png b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..77b85fdbe684bd293863a3890b944028345735ab GIT binary patch literal 6648 zcmVe90{{R30vG`D0000OP)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#17xc2YVlm+Tlfe?1uO8-z)9#za_Zi0WDOgw4~Y-F{x`! z_VeWR+Ur!56!RAA^>C_x#~4Kq*d4kI6wU(gFL)DYK@8oW%)RE=lht^Bbue58%&)1k zKa~5<&u9D*7J)ZW_Rdv2o52ZqH0%lN2h<8KO$1s_CsYNhH|Q?rnsSW!aeZ%$$_5#} zB;9@T;R@{#jSajeUE2XElQt0$U}SvG3DCdY1h5{AepIsG-TbC9Roi&=VFZF%AU9zI>Ub-1 zJpdrg8LDF3FbAF6_`L$cC_fSbqly^)7~AM>CPj)3AD0xl3va_~tERZ`3dYr}6}TW} zO;rcnw18!v)*S;{i6P1~y-R@embfYxla;bH*xhOO4BES5DXaM@Y#19Pn=$@5^LlE+ z=>UcYemlW20|AptQ2;jmxVAA$8s`kg=1x;BX6mjd=3Wmgu*&6`rcZ$SI2V<28QVPr znlT~+rkt-1j8*ymJwgG3pCix<;!u3hlW{Jr@L@)w!&kP3Z3a>lKMoo2^}mIL6LuU5 zxCwaE0{W4*06`!P0qTi~o}dO`j6lZrnn*48W z0Ovh33(T(pshT}e0OFnHTbEmL_!Ndzlt;`+d8Fiu{eROGzEGlkZdD?{^cjK(g{;oaO#{yOYi-s|}M&i}omg{;rG?ZB+N{rd-%1uZ&M%P^BEP%?c z_q&*?cd(fo=cjn2!%N;jv%fbw*uH^_Lv(1^Fg_mbvr=k6tuN)5L?B@suqN?Gl}jtt zFoimquwg_VGwCFq9#AP0hB5w*6a+cx5o%17JSTo-vz1m;h6||Ap0?~l{x>J%UHPWd zHY>CCJ5-7=M(NC1>x>2hOrsM3En}=03Q3@T-4=jrToJ}gtiuRDnOlVMI`qmN>MYTpfMtriV z5MK`P#sNmg8l*=p6vbIvHvQ{H0A(Yr#b6(35QeFNHptgZigIEO>f;%2iD?K-wsUW;F0ICHy-jA{7TLygL z(O%zPp2F7bX#sC6v88`r0;KPqjXkldN%aBA$vCx=Ae+qsSl3uTr^auIYPz5q5BXVi z&(X5_$)8QtJ+KT)R(|2rGiUAhBX)}89Bn`Y!x?LDM0I5!S3sh{=QR%mG*Ep#?#39m z_t7J|CP`5OHo(GJ(w#ANs@3edu_h@kis+x~Fb@>IIBKgr`S}%H!w>4q>2)9Gd#2V(O~!!xa_7d{EpK> zlYT)gosbnj%h&YLW+LE6n|-v|N1J`LnWFEHHj}&lXfuJ`sfyM|n~h=kpVDR|7Z7c) z&ll$7J>JfhKB)nK%9d)JFPUD6n2E6uM+gyI@dE+1?D3WFkAF*afu@TlvKJ--hD(yK z@j&1SA>BhH`(!%cF66WkI^*MFgpoR)?{J+N2sq&^z=!Jp3Dx8C9|!Ys5}=lkGvb8{ zT;Sq`Kc=H&TC*G53O z(2t(#<}u&RUeCTAB?&H)%2i(-fYyK*jHaZMvHNRZMevoyHZEn7;>kW51?5tsIvh*+ z1_5>aCaXe$2?9~uM8j-&$ zdmKbusPuG$8s z@HodJO(z-SBbDR!*-AMHX~Gyyf(xE{SJ_m21otDj-GVzoGtWreP43e%>ANrWKgO8` zD^l#^mh2P@pt9?9$zpQvt>E%1G;QPU9@Zar${mJ`y_!1=cP-yj0RW?yt5FMiAfSQj zmOsqs4%7L=uz6jnD`x^!`DcPfN>(J~5A%xfR>mP+$&^FZ6kgxh8Vgp1VwbyT|+Y+8ZBbr@b9(C#UH`_zY8w$X#6dQ2He@h$)+n^ zPl`X`HZe)8X%Y-d@;vnJJ|Y8nKs(0DT@jN2ZDYg-{6=6zjAS@zaSV+<+H}kIb=Ax@ z@3|=1JVrQw63;DGvx(}qjQ>pum)n*AEf~v`X4-l%(kC%a(q`6=*)<#nDFT`?qM^~$ z!6}+z_+iPM0iDjyEctCmA}Nzk!KG8!)Sht+?J@FcpZ#Mb7%P{+kgjW&z~GPNi1-9R z1J%v6Pt)BlJuW-w#-T4?>2!go_d%6!P9A0ELEo<}h6E~&XCHTWrEy`5#SXC}#8%+M z&XNjyaN@E!Mq12!z!>A)48*vh_@X$>duI5aOy%efHpxN(LrAr* z_aBAW?oXa~DJ73hw9Q(vrXn_+*yLigY4e{<*eeK;p?Vn<;43ZiT_^2lkYv~9I;W5u z+}!aS1t9W~R-E0OGc~zU;T2ZU*9^v4_D;LzecZI`FEMovySZ`K)UC*N&Csszda9dS z4pS7w#F=@9ImzLf#~*cb7e4Op`eyLdIj(s}PN&9w$&yc|$M1kISSSvsSA8wDH75A@ zn;zm|_86GoXS(X8ayF`k$2wh<84d)RuE~7v-xzpTrG=09UVs%|ad7$;DC#okx}K*{ z;Y>4{*HG72oDdyC@qDJ-9;oD>Ilm`=l4T}D)PpJCx8CuQm zXYbsL%Kx#pbz8JR1PPZ*}YWX=~*{~z6&GRYx005 zjM52-evFaAko^@Ho9Erla9+(=kl==9lN5i+N1^pDAKm~6<#6(r=JD#3!E%#(+Q!v* z75i;mQ9t+iDa!O(@cYZK=Tf!{mppstT*&3c7h+OuX=Z|S>PXFlbS+D8?z(N`R=bX4 zpIyVZwqgroL{{DLn+1#+*l?Q<=bA+o-@PM7inD?-Xe!2KCt}#}%)Vnf(J$S3g zkC7*4qW^JFUK%p5rGj@}k&XvcwvH%iiUJ3z5*C6w;7_nF!d8QA`9Zx|R|FG2s5eR)9cajr?I6svvH{ySot*Fg!D!Wg&0vtSB{@i;-IVik~{ zX_v-G6DkLcG4?(XBhDDK01oPU>j3=CR1?g@FvdJdspfucHvl+_n4F_ohwDk^Y;+wG zDw|!$gv!%hmmZ4_{kUegZ_4c$pdaPKJ3Un2ZemrHf;|;W69R5xy=~(dne&|7*=9XP z{&bNab3ew{WzSX{SIyR^yDr#Nm=`)PPC#J^>R~vOgUFf2T~5HFG6!@6^!WDb!?f)S zQx;dcP?Tdh+=*9mXiZ(~6xOXgU&uE77C;$dM;VnQT`1?CecQ4X)e;UPLr&V_&W*dS zDM4d}@6KPSi*BoM10GlURJaVN#>ixSr>mmCj-dl%yS&jBW;y zGuHYo#G=T?d8FSt#Qyw_;r-X1fQZaEpM#V!ig8Z@C`VHJx)yvg1>lANhjb0`oh+?3 zCLmTHV`Ps8aN__C#QeJuTPMSnHA|`?wqCERIE6UVc}xtc6Y$e@SJwdZ-ae7Db6vkf z``7sI&PT?59G@JH^tE{O&!~5d_yJjXmhUhm!$qV{BCgKkuGBgApcPkW~X+OL%6C zWW-$<3lkvYJ9S=xuM~Z^j*(eER`j;-adlycbz<~X7YyVA8!1%TsIGo9q^9fJY_%sT zzK_~#0b0kHuHD*L!e^*G-T7jJJeKEOQzer9lb9;?d~x=}$B!$wj~}-Wzx=bAV~7$e7OL0#)>?R+_i=9&Llq(lr?MQyxJF#%GflHO9v?arKF~ zABLQ6^0SUHjWlB2P3!RHX{Wlz!2+;8iMFog+SCA0xdSk40p4d-Sg3lRyDE51a$c8& zR;~&T{LWj;j#Z(4*2lrDrdqyf_Rdt7E$hk^9Ze#(Ed$=T$ez1*Epe%OhYtz)9bc>v zKqh;Im0ZLryzRy$Vl=rf6V6PicESS2X|m3Bt#p@_Jt3^^z|7!DvO80=D|Jw{M!_2f zi^>E(`gi-67{Kfx^MD?E0SIf>M1~6(^PxTipqA=ODiH5oz2e>W$%tc_lLgKUSm&ZK z;gmKPP*Fewd;BvUp6Kw{GW_ueGyP%Bd@zLLqYo>#iBVIHj*>+kFb41#YMwK>e$SRM z^2wYKfqY2Zj?w=)jgbpgssVMa^y5mme)nO7M;l!b;xKxBQo}FVN_E&~@kku_XufTB zv{8BXVxqnC^PX>QEZe( zoSZZr7^%>5Iea@HNkCBV9{4w$C(FG6hM8fWLOfQN$Ji8~V~iT{iF{O5 zsUCeizB)^zg%FBiF^W#%&XAiHvc0S|d=+^~d#hMFB>+Q||0~ zoj%&)Ps^r48U4M{Rg-U + + + + + + 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:61177478.9 %
Date:2024-01-07 21:47:14Functions: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()13
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)16
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)114
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)226
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)227
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&)458
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2004
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)2572
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&)18340
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)18798
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)19024
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)36008
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)55327
+
+
+ + + +
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..edb02b5fa9 --- /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:61177478.9 %
Date:2024-01-07 21:47:14Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_controllers/src/se3_controller.cpp.gcov.png b/mrs_uav_controllers/src/se3_controller.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..f7bb539044226d891d517c6874f9ea2a4cabe8cb GIT binary patch literal 5825 zcmV;y7Cz~TP)Wh~&aLOZd6^DHK~#Q{%XPY5uk+8p zORx5W??8I3p-g~PP;1w-7O+ph7fj5x3sDS4o-D5G61=tR0u}_Q#;EVwM74sjV&nMO z%0Z9u+}Amb`(pKN(gGGiTL9-ibX`&31)Sa4h`KHU4(NJ1c^Cr(Kd7rV;J%@6$cFtt}>vZ>ss@isRNn?cbd?* zXFR73TOW~t3gG_GZNo^!OtJszlqwCUApgURerMOhOusAvUT7ESkN6KW`Mo~)Ym^>A zhiKzImYxaxY-c#yq=cQh>4+;L#2BX_K__ie+8x(1>Ut7JYwvo%%)l13B?*xEUOO*6 zF;nb!a>{G?tce(v-wdeNP1C?*oKJCs!x%?-33iwNF$Z08CV<{v2s{21AWwB&0=%S+ z9;mtPJS76%iUwjNYlc_?C)Ugi#>grSWLdcZ2!Qw-r^OOc?Uxl8#+k*HW;rSr$=tBu zB@;oOChcM_j8Xh<26$d;_6(1MpM!j~GyFovDB!sEUo#_DVcP7|oHwt+xjxJc@H9yg zLrr)v$yc}f&vi{SdrDc+rL7oa5s-YNW>VF=E)&zfH6PC0EAamLBl9W&CNJtlcAfA` zAIZt7M$4d?>U0Pf{n*zg$4aH^$eW@#T%n~~A~ESSRe986L~>OL#?6|p)eX(H_s^bI zK6GxV{(%457~yQc5m1Ef>#sjBj_}zRTMA00lL;tFX*{zTqh*Yt%Vs)pyWQ3sZsGUs z=U-}<1vmPw1gLJyZTsnCsW#em0Wb_=i9@ddZV`|jh_&k#v!w&ZA{|DIVGp0L(bk!w zu5fMH!@(FOfmk`|(NazEq?xi{k?#5)JY^33Z;OC>jO3)+$On5LYpzo~ zGxg%JWn*^Q0(j@)S2#7|AjHq zXv##C&6b%FT@yEG^f6oWVdL$_p8Fu~xF2Eed82(knK8t%X9GT;@O0r8Cp_iCGke56 z5035peE1*lJ>sG!RXdfSLt`!YrViWHKVz%49k5}|bV?&z@Rq_=8l}1xnCqzy%V+NP5#4_qGG(=$01OXCz|f{j>RjRKvJy{2Tp} z?_vG zxMF+)pdMp`3GjGu(I3|m;PVB*IMo#^!w;_qzoI_uKl*JqPFQufetbMILOzx_=$K^z zRI=yoUDxOFz!Zp3B(~3$0p#sF7?eNo({HX>opS|&`FS`kC1S}j$;F^0xnuWH_{p) z2vQ+HEiqYU>akmaj~XwC^pNb(fPuu?F@S0qA1MPS1{d~}%V9}r__$kfaE(YQ`NePvI1AkH??B0$7&}4dj9e&)83~k%!eit_TpVYi z9AVI97ob^L^MIeqlhU8E>8+3=Cfu-m>VrI zw~rylumNzyK*0ACQ&4GiGqZh(`4S@$qcQjlU3d2&5miC%`cJvyyVcUpPDl6)kj|4% zX*Q#>31cC7rY#$%kB?M8g+og^^zoe|Gc&aZeKOdtIid$Np%S10qeH_C^$AQ7Jw|Wj zdcTBc>NLlyX?7#`&JHWLky`oqIuZ>$@**I%(ZC2AyLMdf3n*HNMxiKYKfPRHnjYgr zm&&2Xcu(VCCZrkTqv{&604lrQ>(tvzm=)KI2Bs7rNmmj-_HJ&h8l1=J}O2I=AXn34Bh_Gqad;6heOnF}H-xMKm- zCR8cecsBqJP+e70_Fne<2GvrOG*8grez#@WZTo*!!0uUgPWcG0Hc5Fy?atJ6Xx7e4 zE#Sg4ISYtLfb{p(rkhPsgHe=f5ehnlzuHu;c@BkkO8l0nrt;jPk;c^7R1m32BaKBE zYd0p5uV5{N0oBCtyIHQ!xLj975$G|i@Z;HYXDbpv?$E!oyR6^LM_^S9swO!%lhJ*maZjQA;k?0{-8vk0RCYv_4K!{goagA5%5-_yY6z z(!HPs?tc^*&Ysu*xR`O~$3gW93)Kq@&{MrA&5tpOY580;WJ9Xm>H~rS7t)~QIjGbZ z*wX-5-T($%syb9N$L5b|H$#v(7PJjf{IK$B#{dt&e-E{phkp**a(Athl)v8lZR1e7Q$<3N^O|;}KU) z(v;7_R&_hhjr6Zv}Qt>7~-!*6WNL^FB`mTrCuKU?0);J_% ze1M0C7vyIsm+)4>1*=)Cqwy8oui%~$+yUzKh;+pmKk1VVcuJjt$MI*XGv^}}K^noS z8Twd4&hKUcRCXOJ>4JB=qMgTB4G^$tVy8#-g zuDL#po)4YtAN0l&ukmF#4e%>v(VEFld_Z8f);1a=&miWJAh$A-z5zsb9oJZaRBVr`ONsF_fcR2|+#tE1C9XvT=eDT?C717lp^Ny z!_4VnOl+9VWjAem#xc~#$X(t6sb^c8;37$7Wvv-w$cTXw#&=EDjR3fR@~eObs;hZq zme;l}Esh}=lcJB718InNW~NJ(G3W|=J@PQxA{C%sLh__G*Gmw_nAw~yc~TT7cE1$Z zgEJw;G13=+cNk-sn~oT9mc9kBQ`cJq;9sU1VLp#BZRKraRBTg&QU-eluP;sUYZ$&x z0RL+Ix_EUbm|yt3c4yqa<7B@0L5e1tkrI2bB4(pHkHsP9Ri%z5@8ydAEnizPdnIaA z{onst*hF`Ey>@N!)9fo^D{#s%j%pRO@>N@|9VX^@zsV*(+ST)t% z($KXdRQcHds{)RDT~wMzP>=as*|J;i$pOC`4kWsTGi^0u1mmi_enN+llvQ{@QFx>m zvcG-@Fk~`5!^7(p-+lF9pu>J0d|<85sK|iUCKL^d zCb(1ycf_H$UGN5g&F~Y&Hfps)8kcKf(c0V(@f3;_PpL+Z-w4B-{KOWd(DrUXjDs(D zerPFA)l#_EM(LV95$NtxVyfNOa*&?k9g1(wLF!&L9r}BnZ2JEZfj?t&$?xdSt0>T4 z-HU2YUpn>wd!`cbF-__PH6JpugEV_WH+_ z6B}va?QS}ZQMI3gf}i$K#F_kU2)t>i-$=3PDvP?2m}ou3!c)9F!Uc!9n$~Aae5Ur& zUpvOQNwDd~?@SoDr_%08GRR406N)={{{$}k|3v7UFcx1pYQ`vDrfSBR?aYx2-e?$+ z2{__(h1S_tduwAQx_7L?rn3<6@fqF%?aZz>7huRN_R|Wx#zoqG&J^Ffr^Cp-qZ=l1 zhDB*GAeHUHrD$>o4s*nbsh84f1!NsX7*W@SBQADbSJ)L@--nmxExF@LV??Yi06wDP z`Xn!)B4RWY%JLU!WiRXY{_*MXx>2TQxT9} z#gxWKGqgI4G4$RMBdMcm0PNKD)&TgIsYaO3W6Yg_4a*f&`;S5Q4$DO+kkqEd_KM#b34ZOrQ;PF|Mt5ehv1`eG0jYK6<(@!Dgw!> zODztK{Ej*_R$Bex@-S`w@R(i`UZBFw2DoQay!vJ8TDM^>`HdX5=`~Vi>@%x*m8E9{ zf8m&bSRi@>lg?BQ85azjZKjVb$if!*ZpoVoiW*jOZRact~-`tr26TV*BB} zcC)+{E4*hGyY#k9K#col08GiBt6wuC!>O>C&WV>2ac&CO8vGO}w9IjFRbT|*t%56i zN#B_#6{Zj+(4^GC7W-QN4g0BpWPu9FkW=C9p&@Jmq_o_Y#l@Bb$ON;CGJLc~jDJZP zPScX+W27=%4Uit2T+jXSR6g#9ZhPOQP;PB@;pLh+QODbbNksXC@-uE6{*^E`jGD>0 zC}+?xg+ofu#@6TdUY6|WS0sM2?Pjc+oQVaWf&g+{gXR7SIqX5g&nI4hT8&cViU7)C zxVEm#Z$wXk8v^XoHN?pbjPae8N; z*heGax9g^^0p>vwpFei6ce<6&9_;zK&K~U8`4g(`sN27$0g&3b97CfOx4<|Yf=YsG zm2gsxFvE;*i1C2dx`j8Uav1yK%>ytaItxX%orU0TNS6zAH!0uCI7Z9c)5SQ-iU0?{ z_4`WR`TZFe5#L;uV@qNoy)brY#3u^&gy!bV%&dOZp1y}fq$jGRoi);YHoE~viLq~c z`8|7@S=Q{x{`y;M}k^`!#ri&)rDQhO1$(|L6QB$q7?NPk2 zWZO4%T`ER83eYyjS_$9t4&O5vVa^9R&(XWD&Ww?axCx_N)^!Ug8!+;EW@Ksea=pev zGUt}->W5e+Mo)FFsY@@HzcKt~00$O;9klI98{c=^s{vZa7*K{6$albq7|#Ieu4x+N zQMfkv3Ku$$PaelB|K6ET@%0SL?dutC!}$Mn&wf3_%g1lS4PyIRcDU4M?4O*Cy}Xk|6`MYe=YQ_ICH&X|m3BsltbqJwEcbf|n-#+!chk$={z?ht93Ned5i^SJ|sE67U89 zd4G=;a7lpDYg`a0cSOK#NpbNCMi4;ff+R-34G_c7Vj82aQYo zNr@{0CeVhsYo$EDEs*1{qdIk6HodOND_Lea0PJGOk@r-H)Q9qz=zciwls?{d-ZhPk z*n{V1W}%@9=Is+s(X?P(2~JL$4vbW2xlFMg5Cv4REwdRgf(Yb**Nrl(5RY|DW5juk z0v}a@mxwXTmg6ugsn+hs-A}H_#p9htMXDhK{oWl<_0?_qw~#!yZK5ubFm_MT0$X!= zx&AE|axd4}jXked{AAjDv=@?2WR8pPlIv1L$2;&>6R=%*|o>MIh zWyl!wLi{X7WzUAL`)>gA1Yaf9V+SA@TdWAuu23s14Ya!S{RZ?O3p;ED_0)>C00000 LNkvXXu0mjfD61f` literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/control_manager/index-detail-sort-f.html b/mrs_uav_managers/include/control_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..34c3a4a8f7 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-01-07 21:47:14Functions: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..ce5e208bf0 --- /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-01-07 21:47:14Functions: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..fe7289247f --- /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-01-07 21:47:14Functions: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..60c4e20302 --- /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-01-07 21:47:14Functions: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..11f5cd6fc3 --- /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-01-07 21:47:14Functions: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..2856d5c6ed --- /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-01-07 21:47:14Functions: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..045acb7fb3 --- /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-01-07 21:47:14Functions: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::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)442
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)49554
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)68303
+
+
+ + + +
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..d4099bf339 --- /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-01-07 21:47:14Functions: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*)68303
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)442
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)49554
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
+
+
+ + + +
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..33c8670b43 --- /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-01-07 21:47:14Functions: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       68303 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52       68303 :     void operator()(const T& msg) {
+      53       68303 :       obj_->publish(msg);
+      54       68303 :     }
+      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-01-07 21:47:14Functions: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&)47
mrs_uav_managers::AglEstimator::~AglEstimator().247
+
+
+ + + +
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..a8ed83ed7e --- /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-01-07 21:47:14Functions: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&)47
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::~AglEstimator().247
+
+
+ + + +
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..b3243b4189 --- /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-01-07 21:47:14Functions: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          47 :   AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      55          47 :       : Estimator(agl::type, name, frame_id), package_name_(package_name) {
+      56          47 :   }
+      57             : 
+      58          47 :   virtual ~AglEstimator(void) {
+      59          47 :   }
+      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..1fd6c9e1be --- /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-01-07 21:47:14Functions: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::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&)442
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)442
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&)483
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
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&)488
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
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&)960
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
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&)1025
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
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&)3752
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&)3774
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5145
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5167
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)6087
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&)7825
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13359
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&)43471
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)51315
+
+
+ + + +
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..b04526dc82 --- /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-01-07 21:47:14Functions: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&)3752
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&)7825
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&)488
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&)442
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&)43471
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&)3774
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&)1025
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&)483
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&)960
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&)6087
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&)5145
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13359
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)442
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)51315
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5167
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
+
+
+ + + +
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..3c43b4cfd4 --- /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-01-07 21:47:14Functions: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        5145 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83        5145 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87        5145 :     double throttle = 0;
+      88             : 
+      89       25725 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90       20580 :       throttle += msg.motors[i];
+      91             :     };
+      92             : 
+      93        5145 :     throttle /= msg.motors.size();
+      94             : 
+      95        5145 :     return throttle;
+      96             :   }
+      97        5167 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98        5167 :     return msg.throttle;
+      99             :   }
+     100       13359 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101       13359 :     return msg.throttle;
+     102             :   }
+     103       51315 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104       51315 :     return msg.throttle;
+     105             :   }
+     106         960 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107         960 :     return std::nullopt;
+     108             :   }
+     109        1025 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110        1025 :     return std::nullopt;
+     111             :   }
+     112         483 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113         483 :     return std::nullopt;
+     114             :   }
+     115         442 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116         442 :     return std::nullopt;
+     117             :   }
+     118         488 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119         488 :     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        3752 :   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        3752 :     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        3752 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153        3774 :   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        3774 :     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        3774 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        7825 :   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        7825 :     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        7825 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173       43471 :   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       43471 :     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       43471 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183         960 :   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         960 :     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         960 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193        1025 :   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        1025 :     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        1025 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203         483 :   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         483 :     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         483 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213         442 :   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         442 :     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         442 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223         488 :   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         488 :     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         488 :     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        6087 :   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        6087 :     initializeHwApiCmd(msg, min_throttle);
+     267        6087 :   }
+     268           0 :   void operator()(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     269             :                   [[maybe_unused]] const double& n_motors) {
+     270           0 :     initializeHwApiCmd(msg, uav_state);
+     271           0 :   }
+     272           0 :   void operator()(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     273             :                   [[maybe_unused]] const double& n_motors) {
+     274           0 :     initializeHwApiCmd(msg, uav_state);
+     275           0 :   }
+     276           0 :   void operator()(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     277             :                   [[maybe_unused]] const double& n_motors) {
+     278           0 :     initializeHwApiCmd(msg, uav_state);
+     279           0 :   }
+     280           0 :   void operator()(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     281             :                   [[maybe_unused]] const double& n_motors) {
+     282           0 :     initializeHwApiCmd(msg, uav_state);
+     283           0 :   }
+     284           0 :   void operator()(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state, [[maybe_unused]] const double& min_throttle,
+     285             :                   [[maybe_unused]] const double& n_motors) {
+     286           0 :     initializeHwApiCmd(msg, uav_state);
+     287           0 :   }
+     288             : };
+     289             : 
+     290             : //}
+     291             : 
+     292             : }  // namespace control_manager
+     293             : 
+     294             : }  // namespace mrs_uav_managers
+     295             : 
+     296             : #endif  // CONTROL_MANAGER_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html new file mode 100644 index 0000000000..29005b23a1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html @@ -0,0 +1,94 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e04229abd45546c00d3c6ae8137138b531e3dcf1 GIT binary patch literal 934 zcmV;X16lluP)KzC z+s6y*mmp8nK% z-EV7y%-;I)I*gkAH1g<|172Pcym+|h$eiKCNn<^-&o$h5VfxVc4ygTF=8ujGs+L-P zN@dM3UarnFceWB3D`T$+fGh&+E(@|o1`RWUqZ+LPwbcQ(4M%A+b4@v|rRK&P>rv4$ zq1%>hEJY$ta`@9?ZQc*W0!=LQJWazyv^UT>m>2Q67zeKFx^3HQQ?{n<4V0O$0g!Cd zWYV1HiF&LfSwENjP{k&g5fkJ{o}KqwF)X#&RCp~F900L||t^=?@ z^__^;HQ8WWXgJZ@VcJ4vFWntVbu3M@D9tI?KOS_4XZDZ4gd5e0`j}KE(B=|laBXPH zyzZwArcDQBzOiN^X9dbUYv!(N=7!3gv}ShKrs2Fu8B86TGTR-Mv7@Rb>ZgowKLjpQ zCa!3|tW2QGPb(AGGpaK3-zWogxl9?X2l!oOB3+)|>>kRr^=7|!zo*9?pZtvi_c7(pihGfgFK(?h}ntV5YL54tTX--L~)0b9+CJE62YoBI*ginmMG5Qhe zw(wptqRaJbG3MS+@PPDPp?f>S88`dX(MD70vCli~1LfuGKNuljxyM8M5dZ)H07*qo IM6N<$g4TMu2mk;8 literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..f4d05ddf0e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-01-07 21:47:14Functions: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..ac76db03c4 --- /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-01-07 21:47:14Functions: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..67be09e4c5 --- /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-01-07 21:47:14Functions: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..f27f59b493 --- /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-01-07 21:47:14Functions: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..09fdf6ac9d --- /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-01-07 21:47:14Functions: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..9b67a2d195 --- /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-01-07 21:47:14Functions: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..5d927c7b3f --- /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-01-07 21:47:14Functions: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().2285
+
+
+ + + +
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..f77191b064 --- /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-01-07 21:47:14Functions: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().2285
+
+
+ + + +
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..95f3d35271 --- /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-01-07 21:47:14Functions: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         285 :   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-01-07 21:47:14Functions: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&)335
mrs_uav_managers::Estimator::~Estimator().2335
+
+
+ + + +
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..6fdb2db609 --- /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-01-07 21:47:14Functions: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&)335
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::~Estimator().2335
+
+
+ + + +
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..c66cf1b683 --- /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-01-07 21:47:14Functions: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         335 :   Estimator(const std::string &type, const std::string &name, const std::string &frame_id) : type_(type), name_(name), frame_id_(frame_id) {
+      64         335 :   }
+      65             : 
+      66         335 :   virtual ~Estimator(void) {
+      67         335 :   }
+      68             : 
+      69             : public:
+      70             :   // virtual methods
+      71             :   virtual void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) = 0;
+      72             :   virtual bool start(void)                                                                                                                = 0;
+      73             :   virtual bool pause(void)                                                                                                                = 0;
+      74             :   virtual bool reset(void)                                                                                                                = 0;
+      75             : 
+      76             :   // implemented methods
+      77             :   // access methods
+      78             :   std::string getName(void) const;
+      79             :   std::string getPrintName(void) const;
+      80             :   std::string getType(void) const;
+      81             :   std::string getFrameId(void) const;
+      82             :   double      getMaxFlightZ(void) const;
+      83             :   std::string getSmStateString(const SMStates_t &state) const;
+      84             :   std::string getCurrentSmStateString(void) const;
+      85             :   SMStates_t  getCurrentSmState() const;
+      86             : 
+      87             :   void setCurrentSmState(const SMStates_t &new_state);
+      88             : 
+      89             :   bool isMitigatingJump() const;
+      90             : 
+      91             :   // state machine methods
+      92             :   bool changeState(SMStates_t new_state);
+      93             :   bool isInState(const SMStates_t &state_in) const;
+      94             :   bool isInitialized() const;
+      95             :   bool isReady() const;
+      96             :   bool isStarted() const;
+      97             :   bool isRunning() const;
+      98             :   bool isStopped() const;
+      99             :   bool isError() const;
+     100             : 
+     101             :   void publishDiagnostics() const;
+     102             : 
+     103             :   tf2::Vector3          getAccGlobal(const sensor_msgs::Imu::ConstPtr &input_msg, const double hdg);
+     104             :   tf2::Vector3          getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr &input_msg, const double hdg);
+     105             :   tf2::Vector3          getAccGlobal(const geometry_msgs::Vector3Stamped &acc_stamped, const double hdg);
+     106             :   std::optional<double> getHeadingRate(const nav_msgs::OdometryConstPtr &odom_msg);
+     107             : };
+     108             : 
+     109             : }  // namespace mrs_uav_managers
+     110             : 
+     111             : #endif  // MRS_UAV_MANAGERS_ESTIMATION_MANAGER_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html new file mode 100644 index 0000000000..1a1e94470f --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html @@ -0,0 +1,48 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..d24a7ae7fa04b7b1633468ba16c7ac71b827e7d9 GIT binary patch literal 505 zcmVHO=r)9+nepA@ zn&O{98B)Arlq6FOQk9AUg8ngfpv^wP-JvRqZctCy9yHThfLoJTHgz4ffeb)&g94s` z>s8=w;TR|)t5PE{+;gdBAlC@?yws^ERrjs#r5HE+=|XeT>L19ax#T;yC*#4z!I+9o zu4G82rk9Z}=q{iDgSN$EfniYhgjwK<>$(d7;yMzd*j%u+Q2y|?OlZFIBasa-nPBc_ zv)>C{3de`+Z0|qiMPJT#BOT}AvgE_QU~~I*HUh(4`?pky2sjp#LMYlHUH7LU4!RX% zK(|$~`Mj^9HC@eXL9P!MNJB8RUn&lY1c~tQ4Gxf^cfs{T_sj#dWGQwI*KU)E*9N#D z%oGF0R}|~^nC6+7Ol^`Akj+>Nxd2Dmd!OMZIgaKl- literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..2a8786366f --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-01-07 21:47:14Functions: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..0d3d31647a --- /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-01-07 21:47:14Functions: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..e2c580114b --- /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-01-07 21:47:14Functions: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..ba26d943cc --- /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-01-07 21:47:14Functions: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..a76e977502 --- /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-01-07 21:47:14Functions: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..ccc40e479b --- /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-01-07 21:47:14Functions: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..313ed06fe9 --- /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-01-07 21:47:14Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)4
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8
mrs_uav_managers::estimation_manager::Support::isStringInVector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)169
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)228
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1347
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)94453
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)177022
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)199074
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)207775
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)208756
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)221407
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)514373
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)531136
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)730477
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)817847
+
+
+ + + +
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..cff501e14d --- /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-01-07 21:47:14Functions: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&)199074
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)4
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)531136
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)730477
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)228
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1347
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)208756
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)94453
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)514373
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)177022
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)207775
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&)169
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)221407
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)817847
+
+
+ + + +
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..a40a02039d --- /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-01-07 21:47:14Functions: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        1347 :   static std::string toSnakeCase(const std::string& str_in) {
+      39             : 
+      40        1347 :     std::string str(1, tolower(str_in[0]));
+      41             : 
+      42       21232 :     for (auto it = str_in.begin() + 1; it != str_in.end(); ++it) {
+      43       19885 :       if (isupper(*it) && *(it - 1) != '_' && islower(*(it - 1))) {
+      44        1118 :         str += "_";
+      45             :       }
+      46       19885 :       str += *it;
+      47             :     }
+      48             : 
+      49        1347 :     std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+      50             : 
+      51        1347 :     return str;
+      52             :   }
+      53             :   /*//}*/
+      54             : 
+      55             : /* toLowercase() //{ */
+      56             : 
+      57         228 : static std::string toLowercase(const std::string str_in) {
+      58         228 :   std::string str_out = str_in;
+      59         228 :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower);
+      60         228 :   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      177022 :   static tf2::Vector3 rotateVecByHdg(const geometry_msgs::Vector3& vec_in, const double hdg_in) {
+      99             : 
+     100      177022 :     const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+     101             : 
+     102      175306 :     const tf2::Vector3 vec_tf2(vec_in.x, vec_in.y, vec_in.z);
+     103             : 
+     104      175253 :     const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+     105             : 
+     106      348478 :     return vec_rotated;
+     107             :   }
+     108             :   //}
+     109             : 
+     110             :   /* noNans() //{ */
+     111      817847 :   static bool noNans(const geometry_msgs::TransformStamped& tf) {
+     112             : 
+     113     2451715 :     return (std::isfinite(tf.transform.rotation.x) && std::isfinite(tf.transform.rotation.y) && std::isfinite(tf.transform.rotation.z) &&
+     114     3267972 :             std::isfinite(tf.transform.rotation.w) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.y) &&
+     115     1634535 :             std::isfinite(tf.transform.translation.z));
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* noNans() //{ */
+     120      221407 :   static bool noNans(const geometry_msgs::Quaternion& q) {
+     121             : 
+     122      221407 :     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      730477 :   static tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) {
+     136             : 
+     137      730477 :     tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z);
+     138             : 
+     139      730235 :     tf2::Quaternion q;
+     140      730183 :     tf2::fromMsg(pose_in.orientation, q);
+     141             : 
+     142      730143 :     tf2::Transform tf_out(q, position);
+     143             : 
+     144     1457948 :     return tf_out;
+     145             :   }
+     146             : 
+     147             :   //}
+     148             : 
+     149             :   /* poseFromTf2() //{ */
+     150             : 
+     151      531136 :   static geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) {
+     152             : 
+     153      531136 :     geometry_msgs::Pose pose_out;
+     154      530160 :     pose_out.position.x = tf_in.getOrigin().getX();
+     155      530294 :     pose_out.position.y = tf_in.getOrigin().getY();
+     156      530621 :     pose_out.position.z = tf_in.getOrigin().getZ();
+     157             : 
+     158      530753 :     pose_out.orientation = tf2::toMsg(tf_in.getRotation());
+     159             : 
+     160      531055 :     return pose_out;
+     161             :   }
+     162             : 
+     163             :   //}
+     164             : 
+     165             :   /* msgFromTf2() //{ */
+     166      199074 :   static geometry_msgs::Transform msgFromTf2(const tf2::Transform& tf_in) {
+     167             : 
+     168      199074 :     geometry_msgs::Transform tf_out;
+     169      199074 :     tf_out.translation.x = tf_in.getOrigin().getX();
+     170      199074 :     tf_out.translation.y = tf_in.getOrigin().getY();
+     171      199074 :     tf_out.translation.z = tf_in.getOrigin().getZ();
+     172      199074 :     tf_out.rotation = tf2::toMsg(tf_in.getRotation());
+     173             : 
+     174      199074 :     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      514373 :   static geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) {
+     194             : 
+     195      514373 :     geometry_msgs::Vector3 vec_out;
+     196      514198 :     vec_out.x = point_in.x;
+     197      514198 :     vec_out.y = point_in.y;
+     198      514198 :     vec_out.z = point_in.z;
+     199             : 
+     200      514198 :     return vec_out;
+     201             :   }
+     202             : 
+     203             :   //}
+     204             : 
+     205             :   /*//{ rotateVector() */
+     206      208756 :   static geometry_msgs::Vector3 rotateVector(const geometry_msgs::Vector3& vec_in, const geometry_msgs::Quaternion& q_in) {
+     207             : 
+     208             :     try {
+     209      208756 :       Eigen::Matrix3d        R = mrs_lib::AttitudeConverter(q_in);
+     210      208754 :       Eigen::Vector3d        vec_in_eigen(vec_in.x, vec_in.y, vec_in.z);
+     211      208437 :       Eigen::Vector3d        vec_eigen_rotated = R * vec_in_eigen;
+     212      208585 :       geometry_msgs::Vector3 vec_out;
+     213      208047 :       vec_out.x = vec_eigen_rotated[0];
+     214      208103 :       vec_out.y = vec_eigen_rotated[1];
+     215      208309 :       vec_out.z = vec_eigen_rotated[2];
+     216      208437 :       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      207775 :   static nav_msgs::Odometry uavStateToOdom(const mrs_msgs::UavState& uav_state) {
+     227      207775 :     nav_msgs::Odometry odom;
+     228      207749 :     odom.header              = uav_state.header;
+     229      207721 :     odom.child_frame_id      = uav_state.child_frame_id;
+     230      207682 :     odom.pose.pose           = uav_state.pose;
+     231      207682 :     odom.twist.twist.angular = uav_state.velocity.angular;
+     232             : 
+     233      207682 :     tf2::Quaternion q;
+     234      207682 :     tf2::fromMsg(odom.pose.pose.orientation, q);
+     235      207682 :     odom.twist.twist.linear = Support::rotateVector(uav_state.velocity.linear, mrs_lib::AttitudeConverter(q.inverse()));
+     236             : 
+     237      414290 :     return odom;
+     238             :   }
+     239             :   /*//}*/
+     240             : 
+     241             :   /*//{ getPoseDiff() */
+     242           4 :   static geometry_msgs::Pose getPoseDiff(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) {
+     243             : 
+     244           4 :     tf2::Vector3 v1, v2;
+     245           4 :     tf2::fromMsg(p1.position, v1);
+     246           4 :     tf2::fromMsg(p2.position, v2);
+     247           4 :     const tf2::Vector3 v3 = v1 - v2;
+     248             : 
+     249           4 :     tf2::Quaternion q1, q2;
+     250           4 :     tf2::fromMsg(p1.orientation, q1);
+     251           4 :     tf2::fromMsg(p2.orientation, q2);
+     252           4 :     tf2::Quaternion q3 = q2 * q1.inverse();
+     253           4 :     q3.normalize();
+     254             : 
+     255           4 :     geometry_msgs::Pose pose_diff;
+     256           4 :     tf2::toMsg(v3, pose_diff.position);
+     257           4 :     pose_diff.orientation = tf2::toMsg(q3);
+     258             : 
+     259           8 :     return pose_diff;
+     260             :   }
+     261             :   /*//}*/
+     262             : 
+     263             :   /*//{ applyPoseDiff() */
+     264       94453 :   static geometry_msgs::Pose applyPoseDiff(const geometry_msgs::Pose& pose_in, const geometry_msgs::Pose& pose_diff) {
+     265             : 
+     266       94453 :     tf2::Vector3    pos_in;
+     267       94453 :     tf2::Quaternion q_in;
+     268       94453 :     tf2::fromMsg(pose_in.position, pos_in);
+     269       94453 :     tf2::fromMsg(pose_in.orientation, q_in);
+     270             : 
+     271       94453 :     tf2::Vector3    pos_diff;
+     272       94453 :     tf2::Quaternion q_diff;
+     273       94453 :     tf2::fromMsg(pose_diff.position, pos_diff);
+     274       94453 :     tf2::fromMsg(pose_diff.orientation, q_diff);
+     275             : 
+     276       94453 :     const tf2::Vector3    pos_out = tf2::quatRotate(q_diff.inverse(), (pos_in - pos_diff));
+     277       94453 :     const tf2::Quaternion q_out   = q_diff.inverse() * q_in;
+     278             : 
+     279       94453 :     geometry_msgs::Pose pose_out;
+     280       94453 :     tf2::toMsg(pos_out, pose_out.position);
+     281       94453 :     pose_out.orientation = tf2::toMsg(q_out);
+     282             : 
+     283      188906 :     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         169 :   static bool isStringInVector(const std::string& value, const std::vector<std::string>& str_vec) {
+     300         169 :     return std::find(str_vec.begin(), str_vec.end(), value) != str_vec.end();
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /*//{ frameIdToEstimatorName() */
+     305           8 :   static std::string frameIdToEstimatorName(const std::string& str_in) {
+     306          16 :     const std::string str_tmp = str_in.substr(str_in.find("/")+1, str_in.size());
+     307          16 :     return str_tmp.substr(0, str_tmp.find("_origin") );
+     308             :   }
+     309             :   /*//}*/
+     310             : 
+     311             : private:
+     312             :   Support() {
+     313             :   }
+     314             : };
+     315             : /*//}*/
+     316             : 
+     317             : }  // namespace estimation_manager
+     318             : 
+     319             : }  // namespace mrs_uav_managers
+     320             : 
+     321             : #endif  // ESTIMATION_MANAGER_SUPPORT_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html new file mode 100644 index 0000000000..4e128092e1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..600621ddc5631c697485f23bb4f61bac03ab2008 GIT binary patch literal 1232 zcmV;>1TXuEP)!aTS z5Vxy9aa$M9sV);`M%x_es-`opvt%zamd49~b&(2;7v*P+*T~+(SeKduCv-i{ynqp} zt`=~5&$R1S-*r3#i7w3&6ffc2>{^8=EOel}ywZVbG_g?7Js zOYN}r?4-J1fl5?sUnT#0=>^cV@jd;UHk^K}S4Vyd4Jhsu~gD~z$WSv=u z(-}ITB2b>r+!Hkg6^)ph9gQKV2U7M^>Y;6?<`EgM0NDh5xxzI|3r{K>X^&q1zIsx? z?q&eEyhkc4iX~{Wz5}I=Nc@!!MDD{sKgmTl0Y<>wzF*tJ58`ZT>%-MJqrFuKM4gpv z`9#;H;BubP5p{v$RVcTfud+wOV|(};Ld}UOBw@S*B#}kG0XQxUNl-ZJnW)eMevv(P z<15cSV0M_Hotv4~0&h&=`+z(HXUIS9&HC9{PB$O}A?e35UiwEgBH*#*5rlozDv;E( z2FKVR-{U)qQs`ND&UP+yDdlJ9;m2k|QdvV0s7cxVZ9rMeo}#<=@fDsTmu{QfRij|_ z6uBOqG$L`f?CtI4e#jP*%jQ5fS`FA5a0l)qW7~2Q5Qn2`dVLG9Z;W>uLWz zf7;CgEfU)E_2TcI1o+SXb{Tt}M&Pg3A5pMuQIj=Obfe)p4Ex8um4U$CaCnR;Jlyt5IPT=mbfITR!t)s$VjotR1Qf{h4W=N?}sNBD{Kl zrB$;+z)x6(on=^Ag=86;aVOAP9CUjDm#wdv}L(8>hN~JiX zO+lPfdvRfM4lya5f;ns~Zn=&_qR-9M!hF>RqWFJA{@Ie9&;jrrA2>9y!lBHRS;F@! zo(XZs4E(pQpZigPK$nIFln5+?CK;ao#J*5WVx2&8)ajD)?Jv6HRJ>?zV& zOtAUFmM=P*2B!e@rv#+7;DsZsSR^>Uhny6g-7DR|)k7)-r}wX?pMR9p{{ShfR7eLX uuF}o73AH3g65Djf>tl2z?7tTjweTN4ektG-6b(NB0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-01-07 21:47:14Functions: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
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html new file mode 100644 index 0000000000..c7be137077 --- /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-01-07 21:47:14Functions: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
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-detail.html b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html new file mode 100644 index 0000000000..179ce7c6e1 --- /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-01-07 21:47:14Functions: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..cc74ac152f --- /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-01-07 21:47:14Functions: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
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html b/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html new file mode 100644 index 0000000000..e7d8f86e19 --- /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-01-07 21:47:14Functions: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
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
state_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/index.html b/mrs_uav_managers/include/mrs_uav_managers/index.html new file mode 100644 index 0000000000..fdef69d535 --- /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-01-07 21:47:14Functions: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..37b7149dba --- /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-01-07 21:47:14Functions: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&)61
mrs_uav_managers::StateEstimator::~StateEstimator().261
+
+
+ + + +
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..17cf0b0630 --- /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-01-07 21:47:14Functions: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&)61
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::~StateEstimator().261
+
+
+ + + +
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..233f744245 --- /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-01-07 21:47:14Functions: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          61 :   StateEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      62          61 :       : Estimator(state::type, name, frame_id), package_name_(package_name) {
+      63          61 :   }
+      64             : 
+      65          61 :   virtual ~StateEstimator(void) {
+      66          61 :   }
+      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..784b4aa413 --- /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-01-07 21:47:14Functions: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().2343
+
+
+ + + +
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..933fd67a3b --- /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-01-07 21:47:14Functions: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().2343
+
+
+ + + +
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..fa5d00779d --- /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-01-07 21:47:14Functions: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         343 :   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:20739852.0 %
Date:2024-01-07 21:47:14Functions:141973.7 %
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 +
86.2%86.2%
+ 		86.2 %207 / 240100.0 %14 / 14
<unnamed>86.2 %207 / 240100.0 %14 / 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..588bf461c1 --- /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:20739852.0 %
Date:2024-01-07 21:47:14Functions:141973.7 %
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 +
86.2%86.2%
+ 		86.2 %207 / 240100.0 %14 / 14
<unnamed>86.2 %207 / 240100.0 %14 / 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..bae9ed2a35 --- /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:20739852.0 %
Date:2024-01-07 21:47:14Functions:141973.7 %
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 +
86.2%86.2%
+ 		86.2 %207 / 240100.0 %14 / 14
<unnamed>86.2 %207 / 240100.0 %14 / 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..b2f8e87395 --- /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:20739852.0 %
Date:2024-01-07 21:47:14Functions:141973.7 %
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 +
86.2%86.2%
+ 		86.2 %207 / 240100.0 %14 / 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..dbf50e3472 --- /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:20739852.0 %
Date:2024-01-07 21:47:14Functions:141973.7 %
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 +
86.2%86.2%
+ 		86.2 %207 / 240100.0 %14 / 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..9414457ee4 --- /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:20739852.0 %
Date:2024-01-07 21:47:14Functions:141973.7 %
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 +
86.2%86.2%
+ 		86.2 %207 / 240100.0 %14 / 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..b555458176 --- /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-01-07 21:47:14Functions: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..947e108c79 --- /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-01-07 21:47:14Functions: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..84c979cde3 --- /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-01-07 21:47:14Functions: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:20724086.2 %
Date:2024-01-07 21:47:14Functions:1414100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::setIsUtmSource(bool)8
mrs_uav_managers::TfSource::getIsUtmSource()16
mrs_uav_managers::TfSource::getIsUtmBased()17
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)61
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)61
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)61
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)61
mrs_uav_managers::TfSource::getName[abi:cxx11]()504
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> > >&)95298
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)113213
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)113268
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)117706
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)117712
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()555388
+
+
+ + + +
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..b3576e2348 --- /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:20724086.2 %
Date:2024-01-07 21:47:14Functions:1414100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()555388
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)61
mrs_uav_managers::TfSource::getIsUtmBased()17
mrs_uav_managers::TfSource::getIsUtmSource()16
mrs_uav_managers::TfSource::publishLocalTf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)61
mrs_uav_managers::TfSource::setIsUtmSource(bool)8
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)61
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)117706
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> > >&)95298
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)113213
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)117712
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)113268
mrs_uav_managers::TfSource::getName[abi:cxx11]()504
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)61
+
+
+ + + +
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..46711a5899 --- /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:20724086.2 %
Date:2024-01-07 21:47:14Functions:1414100.0 %
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          61 :   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          61 :       : name_(name), nh_(nh), broadcaster_(broadcaster), ch_(ch), is_utm_source_(is_utm_source) {
+      42             : 
+      43          61 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      44             : 
+      45          61 :     if (name != "dummy") {
+      46             : 
+      47             :       /*//{ load parameters */
+      48             : 
+      49         122 :       const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+      50             : 
+      51         122 :       std::string odom_topic, attitude_topic, ns;
+      52             : 
+      53          61 :       param_loader->loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
+      54          61 :       param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
+      55          61 :       if (custom_frame_id_enabled_) {
+      56           0 :         param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
+      57             :       }
+      58          61 :       param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
+      59          61 :       if (tf_from_attitude_enabled_) {
+      60          60 :         param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
+      61             :       }
+      62          61 :       param_loader->loadParam(yaml_prefix + getName() + "/namespace", ns);
+      63          61 :       full_topic_odom_     = "/" + ch_->uav_name + "/" + ns + "/" + odom_topic;
+      64          61 :       full_topic_attitude_ = "/" + ch_->uav_name + "/" + ns + "/" + attitude_topic;
+      65          61 :       param_loader->loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
+      66          61 :       param_loader->loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);
+      67             : 
+      68             :       /* coordinate frames origins //{ */
+      69          61 :       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          61 :       if (is_utm_based_) {
+      74         120 :         std::string utm_origin_parent_frame_id;
+      75          60 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
+      76          60 :         ns_utm_origin_parent_frame_id_ = ch_->uav_name + "/" + utm_origin_parent_frame_id;
+      77             : 
+      78          60 :         std::string utm_origin_child_frame_id;
+      79          60 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
+      80          60 :         ns_utm_origin_child_frame_id_ = ch_->uav_name + "/" + utm_origin_child_frame_id;
+      81             :       }
+      82             :       /*//}*/
+      83             : 
+      84             :       /*//{ world source */
+      85          61 :       if (is_utm_based_) {
+      86         120 :         std::string world_origin_parent_frame_id;
+      87          60 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
+      88          60 :         ns_world_origin_parent_frame_id_ = ch_->uav_name + "/" + world_origin_parent_frame_id;
+      89             : 
+      90          60 :         std::string world_origin_child_frame_id;
+      91          60 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
+      92          60 :         ns_world_origin_child_frame_id_ = ch_->uav_name + "/" + world_origin_child_frame_id;
+      93             :       }
+      94             :       /*//}*/
+      95             : 
+      96             :       //}
+      97             : 
+      98          61 :       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         122 :       mrs_lib::SubscribeHandlerOptions shopts;
+     107          61 :       shopts.nh                 = nh_;
+     108          61 :       shopts.node_name          = getPrintName();
+     109          61 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     110          61 :       shopts.threadsafe         = true;
+     111          61 :       shopts.autostart          = true;
+     112          61 :       shopts.queue_size         = 10;
+     113          61 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     114             : 
+     115          61 :       sh_tf_source_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, full_topic_odom_, &TfSource::callbackTfSourceOdom, this);
+     116          61 :       if (tf_from_attitude_enabled_) {
+     117          60 :         sh_tf_source_att_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this);
+     118             :       }
+     119             : 
+     120          61 :       if (is_utm_source_) {
+     121          56 :         sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     122             :       }
+     123             : 
+     124             :     }
+     125             :     /*//}*/
+     126             : 
+     127         114 :     for (auto frame_id : republish_in_frames_) {
+     128          53 :       republishers_.push_back(
+     129         106 :           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          61 :     is_initialized_ = true;
+     132          61 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     133          61 :   }
+     134             :   /*//}*/
+     135             : 
+     136             :   /*//{ getName() */
+     137         504 :   std::string getName() {
+     138         504 :     return name_;
+     139             :   }
+     140             :   /*//}*/
+     141             : 
+     142             :   /*//{ getPrintName() */
+     143      555388 :   std::string getPrintName() {
+     144     1111571 :     return ch_->nodelet_name + "/" + name_;
+     145             :   }
+     146             :   /*//}*/
+     147             : 
+     148             :   /*//{ getIsUtmBased() */
+     149          17 :   bool getIsUtmBased() {
+     150          17 :     return is_utm_based_;
+     151             :   }
+     152             :   /*//}*/
+     153             : 
+     154             :   /*//{ getIsUtmSource() */
+     155          16 :   bool getIsUtmSource() {
+     156          16 :     return is_utm_source_;
+     157             :   }
+     158             :   /*//}*/
+     159             : 
+     160             :   /*//{ setIsUtmSource() */
+     161           8 :   void setIsUtmSource(const bool is_utm_source) {
+     162           8 :     if (is_utm_source) {
+     163           4 :       mrs_lib::SubscribeHandlerOptions shopts;
+     164           4 :       shopts.nh                 = nh_;
+     165           4 :       shopts.node_name          = getPrintName();
+     166           4 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     167           4 :       shopts.threadsafe         = true;
+     168           4 :       shopts.autostart          = true;
+     169           4 :       shopts.queue_size         = 10;
+     170           4 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     171           4 :       sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     172             :     }
+     173           8 :     is_utm_source_ = is_utm_source;
+     174           8 :   }
+     175             :   /*//}*/
+     176             : 
+     177             :   /*//{ setUtmOrigin() */
+     178          61 :   void setUtmOrigin(const geometry_msgs::Point& pt) {
+     179             : 
+     180          61 :     if (is_utm_based_ && !is_utm_origin_set_) {
+     181          60 :       utm_origin_        = pt;
+     182          60 :       is_utm_origin_set_ = true;
+     183             :     }
+     184          61 :   }
+     185             :   /*//}*/
+     186             : 
+     187             :   /*//{ setWorldOrigin() */
+     188          61 :   void setWorldOrigin(const geometry_msgs::Point& pt) {
+     189             : 
+     190          61 :     if (is_utm_based_ && !is_world_origin_set_) {
+     191          60 :       world_origin_        = pt;
+     192          60 :       is_world_origin_set_ = true;
+     193             :     }
+     194          61 :   }
+     195             :   /*//}*/
+     196             : 
+     197             : private:
+     198             :   const std::string name_;
+     199             :   const std::string ns_fcu_frame_id_;
+     200             : 
+     201             :   ros::NodeHandle nh_;
+     202             : 
+     203             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     204             :   tf2_ros::StaticTransformBroadcaster            static_broadcaster_;
+     205             : 
+     206             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     207             : 
+     208             :   bool is_inverted_;
+     209             : 
+     210             :   bool             is_utm_based_;
+     211             :   bool             publish_local_tf_ = true;
+     212             :   bool             publish_utm_tf_   = false;
+     213             :   bool             publish_world_tf_ = false;
+     214             :   std::atomic_bool is_utm_source_    = false;
+     215             :   std::string      ns_utm_origin_parent_frame_id_;
+     216             :   std::string      ns_utm_origin_child_frame_id_;
+     217             : 
+     218             :   bool                 is_utm_origin_set_ = false;
+     219             :   geometry_msgs::Point utm_origin_;
+     220             : 
+     221             :   std::string ns_world_origin_parent_frame_id_;
+     222             :   std::string ns_world_origin_child_frame_id_;
+     223             : 
+     224             :   bool                 is_world_origin_set_ = false;
+     225             :   geometry_msgs::Point world_origin_;
+     226             : 
+     227             :   std::string full_topic_odom_;
+     228             :   std::string full_topic_attitude_;
+     229             :   bool        tf_from_attitude_enabled_ = false;
+     230             : 
+     231             :   bool        custom_frame_id_enabled_;
+     232             :   std::string custom_frame_id_;
+     233             : 
+     234             :   std::atomic_bool is_initialized_               = false;
+     235             :   std::atomic_bool is_local_static_tf_published_ = false;
+     236             :   std::atomic_bool is_utm_static_tf_published_   = false;
+     237             :   std::atomic_bool is_world_static_tf_published_ = false;
+     238             : 
+     239             :   std::vector<std::string> republish_in_frames_;
+     240             : 
+     241             :   std::vector<std::pair<std::string, mrs_lib::PublisherHandler<nav_msgs::Odometry>>> republishers_;
+     242             : 
+     243             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry>               sh_tf_source_odom_;
+     244             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_tf_source_att_;
+     245             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>          sh_altitude_amsl_;
+     246             :   nav_msgs::OdometryConstPtr                                  first_msg_;
+     247             :   bool                                                        got_first_msg_ = false;
+     248             : 
+     249             : 
+     250             :   /*//{ callbackTfSourceOdom()*/
+     251      113268 :   void callbackTfSourceOdom(const nav_msgs::Odometry::ConstPtr msg) {
+     252             : 
+     253      113268 :     if (!is_initialized_) {
+     254           0 :       return;
+     255             :     }
+     256             : 
+     257      339738 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     258             : 
+     259      113218 :     scope_timer.checkpoint("get msg");
+     260             : 
+     261      113249 :     if (!got_first_msg_) {
+     262          61 :       first_msg_     = msg;
+     263          61 :       got_first_msg_ = true;
+     264             :     }
+     265             : 
+     266      113249 :     publishTfFromOdom(msg);
+     267      113276 :     scope_timer.checkpoint("pub tf");
+     268             : 
+     269      113275 :     if (publish_local_tf_ && !is_local_static_tf_published_) {
+     270          61 :       publishLocalTf(msg->header.frame_id);
+     271          61 :       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      208573 :     for (auto republisher : republishers_) {
+     285       95298 :       republishInFrame(msg, ch_->uav_name + "/" + republisher.first, republisher.second);
+     286             :     }
+     287             :   }
+     288             :   /*//}*/
+     289             : 
+     290             :   /*//{ callbackTfSourceAtt()*/
+     291      117712 :   void callbackTfSourceAtt(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     292             : 
+     293      117712 :     if (!is_initialized_) {
+     294           0 :       return;
+     295             :     }
+     296             : 
+     297      353081 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299      117677 :     scope_timer.checkpoint("get msg");
+     300      117688 :     publishTfFromAtt(msg);
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /* publishTfFromOdom() //{*/
+     305      113213 :   void publishTfFromOdom(const nav_msgs::OdometryConstPtr& odom) {
+     306             : 
+     307      226475 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     308             : 
+     309      113261 :     const tf2::Transform      tf       = Support::tf2FromPose(odom->pose.pose);
+     310      113229 :     const tf2::Transform      tf_inv   = tf.inverse();
+     311      113256 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     312             : 
+     313             :     /*//{ tf source origin */
+     314      113228 :     geometry_msgs::TransformStamped tf_msg;
+     315      113161 :     tf_msg.header.stamp         = odom->header.stamp;
+     316      113194 :     std::string origin_frame_id = custom_frame_id_enabled_ ? ch_->uav_name + "/" + custom_frame_id_ : odom->header.frame_id;
+     317      113231 :     if (is_inverted_) {
+     318             : 
+     319      113231 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     320      113224 :       tf_msg.child_frame_id        = origin_frame_id;
+     321      113229 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     322      113246 :       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      113246 :     if (Support::noNans(tf_msg)) {
+     332             :       try {
+     333      113224 :         broadcaster_->sendTransform(tf_msg);
+     334             :       }
+     335           0 :       catch (...) {
+     336           0 :         ROS_ERROR("exception caught ");
+     337             :       }
+     338             : 
+     339      113277 :       if (tf_from_attitude_enabled_) {
+     340      112250 :         if (is_inverted_) {
+     341      112250 :           tf_msg.child_frame_id += "_att_only";
+     342             :         } else {
+     343           0 :           tf_msg.header.frame_id += "_att_only";
+     344             :         }
+     345             :         try {
+     346      112250 :           broadcaster_->sendTransform(tf_msg);
+     347             :         }
+     348           0 :         catch (...) {
+     349           0 :           ROS_ERROR("exception caught ");
+     350             :         }
+     351             :       }
+     352             :       /*//}*/
+     353             : 
+     354             :       /*//{ tf utm origin */
+     355             : 
+     356      113277 :       geometry_msgs::TransformStamped tf_utm_msg;
+     357      113277 :       if (is_utm_source_) {
+     358             :         
+     359       99537 :         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       99537 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     365       99537 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     366       99537 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     367             : 
+     368       99537 :         if (sh_altitude_amsl_.hasMsg()) {
+     369       99536 :           pose_utm.position.z = sh_altitude_amsl_.getMsg()->amsl;
+     370             :         } else {
+     371           1 :           ROS_WARN_THROTTLE(5.0, "[%s]: AMSL altitude not available.", getPrintName().c_str());
+     372             :         }
+     373             : 
+     374       99537 :         tf_utm_msg.header.stamp    = odom->header.stamp;
+     375       99537 :         tf_utm_msg.header.frame_id = ns_utm_origin_parent_frame_id_;
+     376       99537 :         tf_utm_msg.child_frame_id  = ns_utm_origin_child_frame_id_;
+     377             : 
+     378       99537 :         tf2::Transform tf_utm;
+     379       99537 :         if (is_inverted_) {
+     380       99537 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     381             :         } else {
+     382           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     383             :         }
+     384       99537 :         tf_utm_msg.transform = Support::msgFromTf2(tf_utm);
+     385             : 
+     386             :         try {
+     387       99537 :           broadcaster_->sendTransform(tf_utm_msg);
+     388       99537 :           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      113276 :       if (is_utm_source_) {
+     398      199075 :         geometry_msgs::TransformStamped tf_world_msg;
+     399       99537 :         tf_world_msg.header.stamp    = odom->header.stamp;
+     400       99537 :         tf_world_msg.header.frame_id = ns_world_origin_parent_frame_id_;
+     401       99537 :         tf_world_msg.child_frame_id  = ns_world_origin_child_frame_id_;
+     402             : 
+     403       99537 :         tf2::Transform tf_world;
+     404       99537 :         tf_world.setOrigin(tf2::Vector3(world_origin_.x, world_origin_.y, world_origin_.z));
+     405       99537 :         tf_world.setRotation(tf2::Quaternion(0, 0, 0, 1));
+     406             : 
+     407       99537 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     408       99537 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     409       99537 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     410             : 
+     411       99537 :         tf2::Transform tf_utm;
+     412       99537 :         if (is_inverted_) {
+     413       99537 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     414             :         } else {
+     415           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     416             :         }
+     417             : 
+     418       99537 :         tf_world_msg.transform          = Support::msgFromTf2(tf_utm * tf_world);
+     419       99537 :         tf_world_msg.transform.rotation = pose_inv.orientation;
+     420             : 
+     421             :         try {
+     422       99537 :           broadcaster_->sendTransform(tf_world_msg);
+     423       99537 :           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      113274 :     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      117706 :   void publishTfFromAtt(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     443             : 
+     444      353134 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     445             : 
+     446      235419 :     geometry_msgs::TransformStamped tf_msg;
+     447      117670 :     tf_msg.header.stamp = msg->header.stamp;
+     448             : 
+     449      117684 :     geometry_msgs::Pose pose;
+     450      117678 :     pose.position.x = 0.0;
+     451      117678 :     pose.position.y = 0.0;
+     452      117678 :     pose.position.z = 0.0;
+     453      117678 :     pose.orientation = msg->quaternion;
+     454      117690 :     if (is_inverted_) {
+     455             : 
+     456      117690 :       const tf2::Transform      tf       = Support::tf2FromPose(pose);
+     457      117683 :       const tf2::Transform      tf_inv   = tf.inverse();
+     458      117696 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     459             : 
+     460      117709 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     461      117686 :       tf_msg.child_frame_id        = msg->header.frame_id;
+     462      117683 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     463      117693 :       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      117693 :     if (Support::noNans(tf_msg)) {
+     473             :       try {
+     474      117674 :         scope_timer.checkpoint("before pub");
+     475      117679 :         broadcaster_->sendTransform(tf_msg);
+     476      117718 :         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      117718 :     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      117716 :   }
+     488             :   /*//}*/
+     489             : 
+     490             :   /* publishLocalTf() //{*/
+     491          61 :   void publishLocalTf(const std::string& frame_id) {
+     492             : 
+     493         183 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     494             : 
+     495         122 :     geometry_msgs::TransformStamped tf_msg;
+     496          61 :     tf_msg.header.stamp = ros::Time::now();
+     497             : 
+     498          61 :     tf_msg.header.frame_id       = frame_id;
+     499          61 :     tf_msg.child_frame_id        = frame_id.substr(0, frame_id.find("_origin")) + "_local_origin";
+     500          61 :     tf_msg.transform.translation = Support::pointToVector3(first_msg_->pose.pose.position);
+     501          61 :     tf_msg.transform.rotation    = first_msg_->pose.pose.orientation;
+     502             : 
+     503          61 :     if (Support::noNans(tf_msg)) {
+     504             :       try {
+     505          61 :         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          61 :     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          61 :     is_local_static_tf_published_ = true;
+     517          61 :   }
+     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       95298 :   void republishInFrame(const nav_msgs::OdometryConstPtr& msg, const std::string& frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>& ph) {
+     593             : 
+     594      190596 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::republishInFrame", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     595             : 
+     596       95298 :     nav_msgs::Odometry msg_out = *msg;
+     597       95298 :     msg_out.header.frame_id    = frame_id;
+     598             : 
+     599       95298 :     geometry_msgs::PoseStamped pose;
+     600       95298 :     pose.header = msg->header;
+     601       95298 :     pose.pose   = msg->pose.pose;
+     602             : 
+     603       95298 :     auto res = ch_->transformer->transformSingle(pose, frame_id);
+     604       95298 :     if (res) {
+     605       89107 :       msg_out.pose.pose = res->pose;
+     606       89107 :       ph.publish(msg_out);
+     607             :     } else {
+     608        6191 :       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        6191 :       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..d1ca43b648c0d3b9b423cb37eb32ad07df5da778 GIT binary patch literal 2161 zcmV-%2#)uOP)4->0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vppjWjU)pKv3%sDdIM|TkpPY?$fw`;WA#i zHm$a85m>fJz>}S=2Q77h4|*m)YYJ-dH@<5{JAjFK$W-_p0_rJRnH{~Y4pdT%wwX27 zQMJ{IZGhYc>(aMAsaHrYd&IERYcdTmh(XM5(bg?satiE=hH@9WejM$Gwtdf^p;_Sm zP_z>~3O?lYs3Gab%tZ<})+Z3~7~k7{3k~_+!S>j?ZS3`=t@+Sg*P|!F0y$%!wsm1_4UcJ7pg`8x@3!@|^j0?Zl>ULH zxTpR1{Mmo@bRRzV9|WvUaq79SOFgd{Txqu=jWEu^8Fx%`Ve%lufFLkAby^}-fYi4| zpa~RNN9MY20s}>1frNR?!T`1tP}LB{P1-QrM;A!!@)l532LN4zJPK>fxAWMsalU#G z9x}&WBQ$jxq*go>>Uu;_!Gk8H8sWZB8Z8A7{a0{2DSKYs{#t|W(0B-X&AlU-gKK^p zq~6+m>ZN{RsBf;Ck>TXERFXOX{Yr-s@I$1on|r;hmXBEKx>J0F)Qwg1T@{&$t^bhJ z&e1nP1Cw%3!6pm73nYoh`T~g)q-X;(*H$01pD-b5LUVFlpdLeAw>LfOiGVS|Wf_!e-4xnavD!Spuf9i=Eo4h6hk!NfPmkJ5y zE6Y&;@ukXTY%9sVkv+{8mvbY_!bURHA5^r@ASUHOMaM}6SZ9j$6n6bNP?wUaO27d< zw$2M3$gQm#*=xnNr)PORqHtw;HtA8`vFmt5{#YcMHwY|GD*0?7!z71%fgWw~1K97y zAePraxn%ZMEX#x)A_dNt28TRC)z$g1EDR@2xE6%rc`Sqxq`6uca7Y3Qo?70+mNTU4 z2d($K?PQ3~Gcs{H&#(tRrZeJx=b73(17KB(@63nKaG+N7h#iKUC+BfCq{j;_6KNdx z_J=zdHwEzcj6j(pbN-qh=qa8HF|}2#RMz6$rMWOe>@N(yEr98i0G9yeVkDSWta|2X zW$KFTA_ENsmf35p9-#!HIQu{@F(k|p?R6FG4^$I-+&!PMGM0ut0Nf!ER}c}3w&dvf z!!-)r9|X8(iZ6(`c%G>_Mcu)GWlGEPKwZs;Q_pT|lN(Y$kRqSB3+07bUU1Ot1f z4}PUZfL+r^v5FZUTtMoQ)1jBsY&$a{6{@E|$D;|C^gX2S%+;W{g#1!%>RLyNizB{p z9S$f(0ES{YdU#+F0Xz@G0ias*!TN1_GN?fU8u#iZrP#~kXHRii8x7!J38d-<)RVX; zuNgOAFGBYIm!X+hvl5zdZ2ohhS=}y=;X}C;ubsT(+}(wJC2N8zAcJ8i>QM?(ctj}o zk4sJ){%i&`J1%u|a{t~b6P#4lXyb%0TWIS-)4LPCg{E6M(%dNv91=zj1sQ0hI_}p4 zg;^B>Uy<;w=i&vRkc-E_N-qANCZ+Gix(74#tCG@qxo~w-x&*?cG@aspWm1}bwW%w6 zO@RE_;x9>a{~sszx4wojtLAm~HH3{B6Tf$27b=DZx?W?fZvgS6fWQBt5Jq}gd#X(e zP_nTK4cD~{JaraVYUmrJJ~v3wSw>jEend2D1T*l0NUDQ!_h(9aZB51Arh_J{ z)%K0ywIKcpG5o|SfZu|EFN)Od6<(Pv*zT1#tMJChh&%hyWP$6x4vez4C>rs*T*i3Z z_@u)IdkD#b=3z5Ra|eOiz}88H=9s(4u$y~uL7%^J=l%XB0Nb0==>_lj=trS)TwnL) n2faP(+meE{IAL`*Es_5Kcr8{MQa#Eo00000NkvXXu0mjfI + + + + + + 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:14922765.6 %
Date:2024-01-07 21:47:14Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)57
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()57
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)1199
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&)1543
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)12210
+
+
+ + + +
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..de8c8069fe --- /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:14922765.6 %
Date:2024-01-07 21:47:14Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)57
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&)1543
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)1199
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)12210
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)0
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()57
+
+
+ + + +
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..c1d972ed78 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.gcov.html @@ -0,0 +1,706 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/constraint_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - constraint_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:14922765.6 %
Date:2024-01-07 21:47:14Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/constraint_manager.cpp.gcov.png b/mrs_uav_managers/src/constraint_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..637b9ed6eef252b61efd55da7f5e6f6d74642fba GIT binary patch literal 2164 zcmV-)2#fcLP)y?m8!+ z$c*5BC#Eej1bL%sb#%&sH@$VNs)&M)Ai4b;wR%oRG#MOc)Zne1K26hSM=aY}N65?um_Sil6aaL*pW{Ct-9CY?^6ZoBL$M zIL6rNj`7kRG9nq6F#zOxj|7*7=*WQ5MUQwxcu0PXXtmY^}C|8HtDhIk1p0B<6hGNRe6#F#lPJt~a>3I)_DEv$xq%Qr!C z|LNZ77Mr>uZrid!T3$2_%zJ`yTi1=EwH^^tiLnkb`k)?>NL-v>>Rfo`>cLo5TOEV5 zN*OuMdWR?YWk$%k1xC5(=(S}%lapm6W#`5ZFAgGQRKyxYwxA@dW8SzP(2=D?G|RV2 z88N0R&1|l?PpTtYRfvOc$>X)e@Bf^#QT{G*tBgr#St!^Zh`5b4BjPdTba_!UU|*X= zlVgK2)?v0@=&;WrF3qjgG+Xrh$)PZYNS}3!*=+g*rACW!zqD!c7 zQCO>{01I9Gs~M%D`!6A>9SD||93+75@jL>e~e*muF#5ZP3%jv#<1 zag2)5^`YJjc{^JtxD2S&vCBT!O{IVwz9#R;0KujJR43=xb&(n4S+RPn^L(M!n+Kwr zu?5Y3{@543N)c5vC!?&I(Udd6fZMWA=x}ChCb{ttjrh^W`G+oAkyMLWq3I3-ml1wSM$+qHc~$Zkmr(*Z2(WEn8B9RQr%oZW$C z+*W+`m{0U@U(zrIyB7!8@nilc6}V6ySo_DvGuODt)$36S5C>0{&QOeA3 zmWA(VS`oX-YTuw+5#MgCj{_;GtO_wv5j?uOq6*Pg4h2sl#(Tv5ZmHm@qyP@ds%EI8 zP;{CUuqIEemLl4uut4WiKs3*XpMf2^%Kl$cl#+rAA*N0tj0J$E$4x>AD{+T^I7S`f zZwVoE2d{#nYBu9=IOkcY4ASi32E@wjiS0cbAO5c$fK=%3g*voyAUwFwuExJ?NqfO8`Jw8taQy-a^;4t8-+Mlt5 z5}1y&Q}WJ^M8@RJ%e`0<_mnbRCpx)8#oW?RL`D*tTExUZ&Pzs&F4lcxn6^_~5c{<_ z*RCZs-*HU8myF56oT{?O-Lat!7+BY(OcGRlsTDw@Ji*wdWG q + + + + + + 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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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&)57
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&)442
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&)468
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&)483
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&)488
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&)892
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&)960
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&)1025
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2152
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&)3311
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&)3752
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&)3774
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)6087
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&)6087
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)6379
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&)43471
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&)52507
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&)62220
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&)70041
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&)74365
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)84355
+
+
+ + + +
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..e5bf036de6 --- /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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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&)892
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)6379
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)84355
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2152
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&)74365
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&)3311
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&)6087
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&)62220
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&)52507
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&)6087
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&)3752
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&)70041
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&)488
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&)468
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&)57
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&)442
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&)43471
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&)3774
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&)1025
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&)483
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&)960
+
+
+ + + +
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..9d1ffa4f28 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html @@ -0,0 +1,1301 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/common - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22956340.7 %
Date:2024-01-07 21:47:14Functions: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         892 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13        2524 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14        2524 :     if (str == vec[i]) {
+      15         892 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           0 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26       52507 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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       52507 :   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         468 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240         468 :   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         468 :   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         468 :   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         468 :   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         468 :   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         468 :   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         468 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277        3311 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281        3311 :   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        3311 :   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        3311 :   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        3311 :   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        3311 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310       74365 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   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       74365 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460        2152 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462        2152 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464        2152 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466        2152 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471        2152 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472        1684 :     return 0.0;
+     473             :   }
+     474             : 
+     475         468 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477         261 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479         261 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483         207 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485         207 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495          57 : 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         114 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500          57 :   if (custom_config != "") {
+     501          57 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504          57 :   if (platform_config != "") {
+     505          57 :     param_loader.addYamlFile(platform_config);
+     506             :   }
+     507             : 
+     508             :   double arm_length;
+     509             :   double body_height;
+     510             :   double force_constant;
+     511             :   double torque_constant;
+     512             :   double prop_radius;
+     513             :   double rpm_min;
+     514             :   double rpm_max;
+     515             :   double mass;
+     516             : 
+     517          57 :   param_loader.loadParam("uav_mass", mass);
+     518             : 
+     519          57 :   param_loader.loadParam("model_params/arm_length", arm_length);
+     520          57 :   param_loader.loadParam("model_params/body_height", body_height);
+     521             : 
+     522          57 :   param_loader.loadParam("model_params/propulsion/force_constant", force_constant);
+     523          57 :   param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant);
+     524          57 :   param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius);
+     525          57 :   param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min);
+     526          57 :   param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max);
+     527             : 
+     528         171 :   Eigen::MatrixXd allocation_matrix = param_loader.loadMatrixDynamic2("model_params/propulsion/allocation_matrix", 4, -1);
+     529             : 
+     530          57 :   if (!param_loader.loadedSuccessfully()) {
+     531           0 :     ROS_INFO("[%s]: detailed model params not loaded, missing some info", node_name.c_str());
+     532           0 :     return {};
+     533             :   }
+     534             : 
+     535          57 :   int n_motors = allocation_matrix.cols();
+     536             : 
+     537         114 :   DetailedModelParams_t model_params;
+     538             : 
+     539          57 :   model_params.arm_length  = arm_length;
+     540          57 :   model_params.body_height = body_height;
+     541          57 :   model_params.prop_radius = prop_radius;
+     542             : 
+     543         171 :   Eigen::MatrixXd inertia_matrix = param_loader.loadMatrixDynamic2("model_params/inertia_matrix", 3, 3);
+     544             : 
+     545          57 :   if (param_loader.loadedSuccessfully()) {
+     546             : 
+     547           0 :     model_params.inertia = inertia_matrix;
+     548           0 :     ROS_INFO("[%s]: inertia loaded from config file:", node_name.c_str());
+     549           0 :     ROS_INFO_STREAM(model_params.inertia);
+     550             : 
+     551             :   } else {
+     552             : 
+     553             :     // create the inertia matrix
+     554          57 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     555          57 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     556          57 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     557          57 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     558             : 
+     559          57 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     560          57 :     ROS_INFO_STREAM(model_params.inertia);
+     561             :   }
+     562             : 
+     563             :   // create the force-torque allocation matrix
+     564          57 :   model_params.force_torque_mixer = allocation_matrix;
+     565          57 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     566          57 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     567          57 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     568          57 :   model_params.force_torque_mixer.row(3) *= force_constant;
+     569             : 
+     570             :   // | ------- create the control group allocation matrix ------- |
+     571             : 
+     572             :   // pseudoinverse of the force-torque matrix (maximum norm)
+     573             :   Eigen::MatrixXd alloc_tmp =
+     574         114 :       model_params.force_torque_mixer.transpose() * (model_params.force_torque_mixer * model_params.force_torque_mixer.transpose()).inverse();
+     575             : 
+     576             :   // | ------------- normalize the allocation matrix ------------ |
+     577             :   // this will make it match the PX4 control group mixing
+     578             : 
+     579             :   // the first two columns (roll, pitch)
+     580         285 :   for (int i = 0; i < n_motors; i++) {
+     581         228 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     582             :   }
+     583             : 
+     584             :   // the 3rd column (yaw)
+     585         285 :   for (int i = 0; i < n_motors; i++) {
+     586         228 :     if (alloc_tmp(i, 2) > 1e-2) {
+     587         114 :       alloc_tmp(i, 2) = 1.0;
+     588         114 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     589         114 :       alloc_tmp(i, 2) = -1.0;
+     590             :     } else {
+     591           0 :       alloc_tmp(i, 2) = 0.0;
+     592             :     }
+     593             :   }
+     594             : 
+     595             :   // the 4th column (throttle)
+     596         285 :   for (int i = 0; i < n_motors; i++) {
+     597         228 :     alloc_tmp(i, 3) = 1.0;
+     598             :   }
+     599             : 
+     600          57 :   std::cout << "Control group mixer: " << std::endl << alloc_tmp << std::endl;
+     601             : 
+     602          57 :   model_params.control_group_mixer = alloc_tmp;
+     603             : 
+     604          57 :   return model_params;
+     605             : }
+     606             : 
+     607             : //}
+     608             : 
+     609             : /* getLowestOutput() //{ */
+     610             : 
+     611        6379 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     612             : 
+     613        6379 :   if (outputs.actuators) {
+     614          16 :     return ACTUATORS_CMD;
+     615             :   }
+     616             : 
+     617        6363 :   if (outputs.control_group) {
+     618          16 :     return CONTROL_GROUP;
+     619             :   }
+     620             : 
+     621        6347 :   if (outputs.attitude_rate) {
+     622        6281 :     return ATTITUDE_RATE;
+     623             :   }
+     624             : 
+     625          66 :   if (outputs.attitude) {
+     626          16 :     return ATTITUDE;
+     627             :   }
+     628             : 
+     629          50 :   if (outputs.acceleration_hdg_rate) {
+     630          10 :     return ACCELERATION_HDG_RATE;
+     631             :   }
+     632             : 
+     633          40 :   if (outputs.acceleration_hdg) {
+     634          10 :     return ACCELERATION_HDG;
+     635             :   }
+     636             : 
+     637          30 :   if (outputs.velocity_hdg_rate) {
+     638          10 :     return VELOCITY_HDG_RATE;
+     639             :   }
+     640             : 
+     641          20 :   if (outputs.velocity_hdg) {
+     642          10 :     return VELOCITY_HDG;
+     643             :   }
+     644             : 
+     645          10 :   return POSITION;
+     646             : }
+     647             : 
+     648             : //}
+     649             : 
+     650             : /* getHighestOutput() //{ */
+     651             : 
+     652           0 : CONTROL_OUTPUT getHighestOuput(const ControlOutputModalities_t& outputs) {
+     653             : 
+     654           0 :   if (outputs.position) {
+     655           0 :     return POSITION;
+     656             :   }
+     657             : 
+     658           0 :   if (outputs.velocity_hdg) {
+     659           0 :     return VELOCITY_HDG;
+     660             :   }
+     661             : 
+     662           0 :   if (outputs.velocity_hdg_rate) {
+     663           0 :     return VELOCITY_HDG_RATE;
+     664             :   }
+     665             : 
+     666           0 :   if (outputs.acceleration_hdg) {
+     667           0 :     return ACCELERATION_HDG;
+     668             :   }
+     669             : 
+     670           0 :   if (outputs.acceleration_hdg_rate) {
+     671           0 :     return ACCELERATION_HDG_RATE;
+     672             :   }
+     673             : 
+     674           0 :   if (outputs.attitude) {
+     675           0 :     return ATTITUDE;
+     676             :   }
+     677             : 
+     678           0 :   if (outputs.attitude_rate) {
+     679           0 :     return ATTITUDE_RATE;
+     680             :   }
+     681             : 
+     682           0 :   if (outputs.control_group) {
+     683           0 :     return CONTROL_GROUP;
+     684             :   }
+     685             : 
+     686           0 :   return ACTUATORS_CMD;
+     687             : }
+     688             : 
+     689             : //}
+     690             : 
+     691             : // | -------- extraction of throttle out of hw api cmds ------- |
+     692             : 
+     693             : /* extractThrottle() //{ */
+     694             : 
+     695       84355 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     696             : 
+     697       84355 :   if (!control_output.control_output) {
+     698        5971 :     return {};
+     699             :   }
+     700             : 
+     701      156768 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     702             : }
+     703             : 
+     704             : //}
+     705             : 
+     706             : // | -------------- validation of HW api commands ------------- |
+     707             : 
+     708             : /* validateControlOutput() //{ */
+     709             : 
+     710       62220 : bool validateControlOutput(const Controller::ControlOutput& control_output, const ControlOutputModalities_t& output_modalities, const std::string& node_name,
+     711             :                            const std::string& var_name) {
+     712             : 
+     713       62220 :   if (!control_output.control_output) {
+     714           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: the optional variable '%s' is not set!!!", node_name.c_str(), var_name.c_str());
+     715           0 :     return false;
+     716             :   }
+     717             : 
+     718       62220 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     719      124440 :   std::variant<std::string>               node_name_var{node_name};
+     720       62220 :   std::variant<std::string>               var_name_var{var_name};
+     721             : 
+     722       62220 :   return std::visit(HwApiValidateVisitor(), control_output.control_output.value(), output_modalities_var, node_name_var, var_name_var);
+     723             : }
+     724             : 
+     725             : //}
+     726             : 
+     727             : /* validateHwApiActuatorCmd() //{ */
+     728             : 
+     729        3752 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     730             : 
+     731       18760 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     732       15008 :     if (!std::isfinite(msg.motors[i])) {
+     733           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.motors[%d]'!!!", node_name.c_str(), var_name.c_str(), int(i));
+     734           0 :       return false;
+     735             :     }
+     736             :   }
+     737             : 
+     738        3752 :   return true;
+     739             : }
+     740             : 
+     741             : //}
+     742             : 
+     743             : /* validateHwApiControlGroupCmd() //{ */
+     744             : 
+     745        3774 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     746             : 
+     747        3774 :   if (!std::isfinite(msg.roll)) {
+     748           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.roll'!!!", node_name.c_str(), var_name.c_str());
+     749           0 :     return false;
+     750             :   }
+     751             : 
+     752        3774 :   if (!std::isfinite(msg.pitch)) {
+     753           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.pitch'!!!", node_name.c_str(), var_name.c_str());
+     754           0 :     return false;
+     755             :   }
+     756             : 
+     757        3774 :   if (!std::isfinite(msg.yaw)) {
+     758           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.yaw'!!!", node_name.c_str(), var_name.c_str());
+     759           0 :     return false;
+     760             :   }
+     761             : 
+     762        3774 :   if (!std::isfinite(msg.throttle)) {
+     763           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     764           0 :     return false;
+     765             :   }
+     766             : 
+     767        3774 :   return true;
+     768             : }
+     769             : 
+     770             : //}
+     771             : 
+     772             : /* validateHwApiAttitudeCmd() //{ */
+     773             : 
+     774       70041 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     775             : 
+     776             :   // check the orientation
+     777             : 
+     778       70041 :   if (!std::isfinite(msg.orientation.x)) {
+     779           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.x'!!!", node_name.c_str(), var_name.c_str());
+     780           0 :     return false;
+     781             :   }
+     782             : 
+     783       70041 :   if (!std::isfinite(msg.orientation.y)) {
+     784           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.y'!!!", node_name.c_str(), var_name.c_str());
+     785           0 :     return false;
+     786             :   }
+     787             : 
+     788       70041 :   if (!std::isfinite(msg.orientation.z)) {
+     789           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.z'!!!", node_name.c_str(), var_name.c_str());
+     790           0 :     return false;
+     791             :   }
+     792             : 
+     793       70041 :   if (!std::isfinite(msg.orientation.w)) {
+     794           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.orientation.w'!!!", node_name.c_str(), var_name.c_str());
+     795           0 :     return false;
+     796             :   }
+     797             : 
+     798             :   // check the throttle
+     799             : 
+     800       70041 :   if (!std::isfinite(msg.throttle)) {
+     801           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     802           0 :     return false;
+     803             :   }
+     804             : 
+     805       70041 :   return true;
+     806             : }
+     807             : 
+     808             : //}
+     809             : 
+     810             : /* validateHwApiAttitudeRateCmd() //{ */
+     811             : 
+     812       43471 : bool validateHwApiAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     813             : 
+     814             :   // check the body rate
+     815             : 
+     816       43471 :   if (!std::isfinite(msg.body_rate.x)) {
+     817           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.x'!!!", node_name.c_str(), var_name.c_str());
+     818           0 :     return false;
+     819             :   }
+     820             : 
+     821       43471 :   if (!std::isfinite(msg.body_rate.y)) {
+     822           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.y'!!!", node_name.c_str(), var_name.c_str());
+     823           0 :     return false;
+     824             :   }
+     825             : 
+     826       43471 :   if (!std::isfinite(msg.body_rate.z)) {
+     827           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.body_rate.z'!!!", node_name.c_str(), var_name.c_str());
+     828           0 :     return false;
+     829             :   }
+     830             : 
+     831             :   // check the throttle
+     832             : 
+     833       43471 :   if (!std::isfinite(msg.throttle)) {
+     834           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.throttle'!!!", node_name.c_str(), var_name.c_str());
+     835           0 :     return false;
+     836             :   }
+     837             : 
+     838       43471 :   return true;
+     839             : }
+     840             : 
+     841             : //}
+     842             : 
+     843             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     844             : 
+     845         960 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     846             : 
+     847             :   // | ----------------- check the acceleration ----------------- |
+     848             : 
+     849         960 :   if (!std::isfinite(msg.acceleration.x)) {
+     850           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     851           0 :     return false;
+     852             :   }
+     853             : 
+     854         960 :   if (!std::isfinite(msg.acceleration.y)) {
+     855           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     856           0 :     return false;
+     857             :   }
+     858             : 
+     859         960 :   if (!std::isfinite(msg.acceleration.z)) {
+     860           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     861           0 :     return false;
+     862             :   }
+     863             : 
+     864             :   // check the heading rate
+     865             : 
+     866         960 :   if (!std::isfinite(msg.heading_rate)) {
+     867           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     868           0 :     return false;
+     869             :   }
+     870             : 
+     871         960 :   return true;
+     872             : }
+     873             : 
+     874             : //}
+     875             : 
+     876             : /* validateHwApiAccelerationHdgCmd() //{ */
+     877             : 
+     878        1025 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     879             : 
+     880             :   // | ----------------- check the acceleration ----------------- |
+     881             : 
+     882        1025 :   if (!std::isfinite(msg.acceleration.x)) {
+     883           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.x'!!!", node_name.c_str(), var_name.c_str());
+     884           0 :     return false;
+     885             :   }
+     886             : 
+     887        1025 :   if (!std::isfinite(msg.acceleration.y)) {
+     888           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.y'!!!", node_name.c_str(), var_name.c_str());
+     889           0 :     return false;
+     890             :   }
+     891             : 
+     892        1025 :   if (!std::isfinite(msg.acceleration.z)) {
+     893           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.acceleration.z'!!!", node_name.c_str(), var_name.c_str());
+     894           0 :     return false;
+     895             :   }
+     896             : 
+     897             :   // check the heading
+     898             : 
+     899        1025 :   if (!std::isfinite(msg.heading)) {
+     900           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     901           0 :     return false;
+     902             :   }
+     903             : 
+     904        1025 :   return true;
+     905             : }
+     906             : 
+     907             : //}
+     908             : 
+     909             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     910             : 
+     911         483 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     912             : 
+     913             :   // | ----------------- check the velocity ----------------- |
+     914             : 
+     915         483 :   if (!std::isfinite(msg.velocity.x)) {
+     916           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     917           0 :     return false;
+     918             :   }
+     919             : 
+     920         483 :   if (!std::isfinite(msg.velocity.y)) {
+     921           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     922           0 :     return false;
+     923             :   }
+     924             : 
+     925         483 :   if (!std::isfinite(msg.velocity.z)) {
+     926           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     927           0 :     return false;
+     928             :   }
+     929             : 
+     930             :   // check the heading rate
+     931             : 
+     932         483 :   if (!std::isfinite(msg.heading_rate)) {
+     933           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading_rate'!!!", node_name.c_str(), var_name.c_str());
+     934           0 :     return false;
+     935             :   }
+     936             : 
+     937         483 :   return true;
+     938             : }
+     939             : 
+     940             : //}
+     941             : 
+     942             : /* validateHwApiVelocityHdgCmd() //{ */
+     943             : 
+     944         442 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     945             : 
+     946             :   // | ----------------- check the velocity ----------------- |
+     947             : 
+     948         442 :   if (!std::isfinite(msg.velocity.x)) {
+     949           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.x'!!!", node_name.c_str(), var_name.c_str());
+     950           0 :     return false;
+     951             :   }
+     952             : 
+     953         442 :   if (!std::isfinite(msg.velocity.y)) {
+     954           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.y'!!!", node_name.c_str(), var_name.c_str());
+     955           0 :     return false;
+     956             :   }
+     957             : 
+     958         442 :   if (!std::isfinite(msg.velocity.z)) {
+     959           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.velocity.z'!!!", node_name.c_str(), var_name.c_str());
+     960           0 :     return false;
+     961             :   }
+     962             : 
+     963             :   // check the heading
+     964             : 
+     965         442 :   if (!std::isfinite(msg.heading)) {
+     966           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+     967           0 :     return false;
+     968             :   }
+     969             : 
+     970         442 :   return true;
+     971             : }
+     972             : 
+     973             : //}
+     974             : 
+     975             : /* validateHwApiPositionHdgCmd() //{ */
+     976             : 
+     977         488 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     978             : 
+     979             :   // | ----------------- check the position ----------------- |
+     980             : 
+     981         488 :   if (!std::isfinite(msg.position.x)) {
+     982           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.x'!!!", node_name.c_str(), var_name.c_str());
+     983           0 :     return false;
+     984             :   }
+     985             : 
+     986         488 :   if (!std::isfinite(msg.position.y)) {
+     987           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.y'!!!", node_name.c_str(), var_name.c_str());
+     988           0 :     return false;
+     989             :   }
+     990             : 
+     991         488 :   if (!std::isfinite(msg.position.z)) {
+     992           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.position.z'!!!", node_name.c_str(), var_name.c_str());
+     993           0 :     return false;
+     994             :   }
+     995             : 
+     996             :   // check the heading
+     997             : 
+     998         488 :   if (!std::isfinite(msg.heading)) {
+     999           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in variable '%s.heading'!!!", node_name.c_str(), var_name.c_str());
+    1000           0 :     return false;
+    1001             :   }
+    1002             : 
+    1003         488 :   return true;
+    1004             : }
+    1005             : 
+    1006             : //}
+    1007             : 
+    1008             : // | ------------ initialization of HW api commands ----------- |
+    1009             : 
+    1010             : /* initializeDefaultOutput() //{ */
+    1011             : 
+    1012        6087 : Controller::HwApiOutputVariant initializeDefaultOutput(const ControlOutputModalities_t& possible_outputs, const mrs_msgs::UavState& uav_state,
+    1013             :                                                        const double& min_throttle, const double& n_motors) {
+    1014             : 
+    1015        6087 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1016             : 
+    1017        6087 :   Controller::HwApiOutputVariant output;
+    1018             : 
+    1019        6087 :   switch (lowest_output) {
+    1020           0 :     case ACTUATORS_CMD: {
+    1021           0 :       output = mrs_msgs::HwApiActuatorCmd();
+    1022           0 :       break;
+    1023             :     }
+    1024           0 :     case CONTROL_GROUP: {
+    1025           0 :       output = mrs_msgs::HwApiControlGroupCmd();
+    1026           0 :       break;
+    1027             :     }
+    1028        6087 :     case ATTITUDE_RATE: {
+    1029        6087 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1030        6087 :       break;
+    1031             :     }
+    1032           0 :     case ATTITUDE: {
+    1033           0 :       output = mrs_msgs::HwApiAttitudeCmd();
+    1034           0 :       break;
+    1035             :     }
+    1036           0 :     case ACCELERATION_HDG_RATE: {
+    1037           0 :       output = mrs_msgs::HwApiAccelerationHdgRateCmd();
+    1038           0 :       break;
+    1039             :     }
+    1040           0 :     case ACCELERATION_HDG: {
+    1041           0 :       output = mrs_msgs::HwApiAccelerationHdgCmd();
+    1042           0 :       break;
+    1043             :     }
+    1044           0 :     case VELOCITY_HDG_RATE: {
+    1045           0 :       output = mrs_msgs::HwApiVelocityHdgRateCmd();
+    1046           0 :       break;
+    1047             :     }
+    1048           0 :     case VELOCITY_HDG: {
+    1049           0 :       output = mrs_msgs::HwApiVelocityHdgCmd();
+    1050           0 :       break;
+    1051             :     }
+    1052           0 :     case POSITION: {
+    1053           0 :       output = mrs_msgs::HwApiPositionCmd();
+    1054           0 :       break;
+    1055             :     }
+    1056             :   }
+    1057             : 
+    1058       12174 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1059        6087 :   std::variant<double>             min_throttle_var{min_throttle};
+    1060        6087 :   std::variant<double>             n_motors_var{n_motors};
+    1061             : 
+    1062        6087 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1063             : 
+    1064       12174 :   return output;
+    1065             : }  // namespace mrs_uav_managers
+    1066             : 
+    1067             : //}
+    1068             : 
+    1069             : /* initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg) //{ */
+    1070             : 
+    1071           0 : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors) {
+    1072             : 
+    1073           0 :   msg.stamp = ros::Time::now();
+    1074             : 
+    1075           0 :   for (int i = 0; i < n_motors; i++) {
+    1076           0 :     msg.motors.push_back(min_throttle);
+    1077             :   }
+    1078           0 : }
+    1079             : 
+    1080             : //}
+    1081             : 
+    1082             : /* initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg) //{ */
+    1083             : 
+    1084           0 : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle) {
+    1085             : 
+    1086           0 :   msg.stamp = ros::Time::now();
+    1087             : 
+    1088           0 :   msg.roll     = 0;
+    1089           0 :   msg.pitch    = 0;
+    1090           0 :   msg.yaw      = 0;
+    1091           0 :   msg.throttle = min_throttle;
+    1092           0 : }
+    1093             : 
+    1094             : //}
+    1095             : 
+    1096             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg) //{ */
+    1097             : 
+    1098        6087 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1099             : 
+    1100        6087 :   msg.stamp = ros::Time::now();
+    1101             : 
+    1102        6087 :   msg.body_rate.x = 0;
+    1103        6087 :   msg.body_rate.y = 0;
+    1104        6087 :   msg.body_rate.z = 0;
+    1105        6087 :   msg.throttle    = min_throttle;
+    1106        6087 : }
+    1107             : 
+    1108             : //}
+    1109             : 
+    1110             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg) //{ */
+    1111             : 
+    1112           0 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeCmd& msg, const mrs_msgs::UavState& uav_state, const double& min_throttle) {
+    1113             : 
+    1114           0 :   msg.stamp = ros::Time::now();
+    1115             : 
+    1116           0 :   msg.orientation = uav_state.pose.orientation;
+    1117           0 :   msg.throttle    = min_throttle;
+    1118           0 : }
+    1119             : 
+    1120             : //}
+    1121             : 
+    1122             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg) //{ */
+    1123             : 
+    1124           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1125             : 
+    1126           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1127           0 :   msg.header.stamp    = ros::Time::now();
+    1128             : 
+    1129           0 :   msg.acceleration.x = 0;
+    1130           0 :   msg.acceleration.y = 0;
+    1131           0 :   msg.acceleration.z = 0;
+    1132           0 :   msg.heading_rate   = 0;
+    1133           0 : }
+    1134             : 
+    1135             : //}
+    1136             : 
+    1137             : /* initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg) //{ */
+    1138             : 
+    1139           0 : void initializeHwApiCmd(mrs_msgs::HwApiAccelerationHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1140             : 
+    1141           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1142           0 :   msg.header.stamp    = ros::Time::now();
+    1143             : 
+    1144           0 :   msg.acceleration.x = 0;
+    1145           0 :   msg.acceleration.y = 0;
+    1146           0 :   msg.acceleration.z = 0;
+    1147             : 
+    1148             :   try {
+    1149           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1150             :   }
+    1151           0 :   catch (std::runtime_error& exrun) {
+    1152           0 :     msg.heading = 0;
+    1153             :   }
+    1154           0 : }
+    1155             : 
+    1156             : //}
+    1157             : 
+    1158             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg) //{ */
+    1159             : 
+    1160           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1161             : 
+    1162           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1163           0 :   msg.header.stamp    = ros::Time::now();
+    1164             : 
+    1165           0 :   msg.velocity.x   = 0;
+    1166           0 :   msg.velocity.y   = 0;
+    1167           0 :   msg.velocity.z   = 0;
+    1168           0 :   msg.heading_rate = 0;
+    1169           0 : }
+    1170             : 
+    1171             : //}
+    1172             : 
+    1173             : /* initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg) //{ */
+    1174             : 
+    1175           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1176             : 
+    1177           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1178           0 :   msg.header.stamp    = ros::Time::now();
+    1179             : 
+    1180           0 :   msg.velocity.x = 0;
+    1181           0 :   msg.velocity.y = 0;
+    1182           0 :   msg.velocity.z = 0;
+    1183             : 
+    1184             :   try {
+    1185           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1186             :   }
+    1187           0 :   catch (std::runtime_error& exrun) {
+    1188           0 :     msg.heading = 0;
+    1189             :   }
+    1190           0 : }
+    1191             : 
+    1192             : //}
+    1193             : 
+    1194             : /* initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg) //{ */
+    1195             : 
+    1196           0 : void initializeHwApiCmd(mrs_msgs::HwApiPositionCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1197             : 
+    1198           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1199           0 :   msg.header.stamp    = ros::Time::now();
+    1200             : 
+    1201           0 :   msg.position.x = uav_state.pose.position.x;
+    1202           0 :   msg.position.y = uav_state.pose.position.y;
+    1203           0 :   msg.position.z = uav_state.pose.position.z;
+    1204             : 
+    1205             :   try {
+    1206           0 :     msg.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1207             :   }
+    1208           0 :   catch (std::runtime_error& exrun) {
+    1209           0 :     msg.heading = 0;
+    1210             :   }
+    1211           0 : }
+    1212             : 
+    1213             : //}
+    1214             : 
+    1215             : }  // namespace control_manager
+    1216             : 
+    1217             : }  // namespace mrs_uav_managers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html new file mode 100644 index 0000000000..d11dc2de30 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html @@ -0,0 +1,325 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common/common.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png b/mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..54755d07874d86e69c202725a8548a8d08566527 GIT binary patch literal 2398 zcmV-k38D6hP)`6pH zRCt{2U5%luDh#e0*cIvqxOjzKQ!;p=QDra(e_y4G=h-a!bW(e8NS-De=c*JreT6@oRGGT zaEh6&937)V2;bY#sJ86GG|Q{?V$mN3;d&h#p~sszhKU;QuEayTy)1?MoXoEttt8QY zsU8MXE11J@a|NMWsfWST3g$4}Trn3>52~Oh)WcwE1tSeN24=Yi}e zG>|7YhV38v6B^&x_~7}6ChU>HGy~>VS+15-@kn9_9h*kX?3S62To0$36VP$3CL(#w z6H-2VWH5Dt`NEBywr7P~2GU$9WFwG{@XCmzl?77VCYx|q22(4T{3&GozNams7gnAfEtzay}&5$e!t7mA-uNh3eVE8UTpiRjA z)Inta<$pw^N~yr2S&Aw@!&c?T9f8D}E<%!EO(1QL%MyN|>+#wcse2xql(G9<8^@4o zmW#OYoPwdA1DE&&-IZ7`X(c530v!vKF;vwl!6*cn!N9&7$o|a;2-7(3_8K81a(t{) zWtTAG1U;IlO%Iz6Crd|-xu{8)-6$SU;>qs<71K&TdYK_UBU3LrAxFnr7i%N8m)oac zVeh$UCSk8^M;%MF&R|k8OE(l_ z50V`_qRX8*NobUw4X_S&i2^A#tQD8b@ zs*Iz_G_#qZ2j0mX;f{S)r+82^gN7@%_Tq1N^4K7R4d+NT-0Z7nu{!H@%GK`*z$#XC z86yad!(0K3Y5^{{9^*0mR|riiwLVW=*(-w-F0S1XqQaLmD8~ZJ89gv+(SQ$-Eg}fv zmW&9N0VrDiA`2nhapH5~L1>H&Rgce3-sRH9+bd1&pVPN7x>hvQdjE9SO?o2}>F4EH z4cn7!GHPWI-C-AURjH-fwvYQBQsqjdwz|_dRRj;s$Qv#8Npl#6Q6?U|#WsL;hH-+1 zWfK;0u1=x|sv`xAO6_f=-a5;NQ4Ha{!d9Cbq^x#V!jJBl9R0eI9Q|Et!nc~b=5W4n zC8Vvn3YB*6w1)=vHOclE8JIHrYca+`zCFb}jK_D%46f{FtmGd+oxsuDU3m!Gx?Pa# z%>YWxN0c|!Kb^Q;a4?HxhL4Uz$}gH23QNf{l4T5*=m#=Jx-P9pzb*|uhICn8#?Ukj zWejv(UXMPDv}6oT>}6#PSfn9iuD-oCLg7X}6^G1E_A|R5Y=c zg{rq0$Fdbz0#X7QBX`8hSBaT@9HES%yX@s<^lH+QG0^ols_o#Kv}E*avaF14_>9*Y zEj@nunU6MPB=y`%!K6+>@3|&pv*qr)F5Oq~R6&sI9smH|> z&>LCzbRe6mEKDD~xnyR!EX`K;fa!d+&CIM7V3-lT>+4lHKH>&d4KHoOvfqq>FsU{Mbr7AGqL@o&oe6*+nFs+wNyls%GbJ7GS zAPN=)Y4!~w42Hpc0Sx&Z(s}jptN;ng?SC#^wLO-6?_RraFlI2dg5f|K0dwO(dOb_) z$%Z>$o_Wmn2^ZSE4*to~rXP!`*j*V+tzbYP&Gh1Eet|%cklelzNUz@PLIWwXNaaA9 z%^n#{onSy9P54ogdiVtbkdWLskRE%b^I`O6@B3gdwSwUlX#~uT1L^gLuaKze-bzco zbH;&`!L$d4SELaz97qGL$Rh|GNEu9fU^tLQz}za3nh8hic)``v1zC<4Se|8@lD-Os zW_i{?R@))Bm)VJ1mP-vGaj4z06T{@IMwY@+$rY@Eu=$KSeHA%rw|kd$cF;>ma4z!m z;TL6eg()E8c7Q?^L5P$hc!i9{CZvot@vzjPNB=>dIU(&}K8yhd79vggg&U77~K}r%vCa>nr?2M5`%uE{@n^}~QD&vEi zPL>OHONnlyFWBG4&6U}kCIMZ?IR1~l0y`Jr7~2FCB#by=>t=S&$bx1DjI1#;i)}Sa z5K?927#`+GCH%*Gu3tNsf#fTMR2kX1!W^lD`JJm51wPeN5*heg9KCbjAH54>m<&+? QJOBUy07*qoM6N<$f)HMRfdBvi literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html b/mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html new file mode 100644 index 0000000000..12d130fc15 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/common + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager/commonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22956340.7 %
Date:2024-01-07 21:47:14Functions: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 +
40.7%40.7%
+ 		40.7 %229 / 56367.7 %21 / 31
<unnamed>40.7 %229 / 56367.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..2302e89d1c --- /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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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 +
40.7%40.7%
+ 		40.7 %229 / 56367.7 %21 / 31
<unnamed>40.7 %229 / 56367.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..2f8cc270b7 --- /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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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 +
40.7%40.7%
+ 		40.7 %229 / 56367.7 %21 / 31
<unnamed>40.7 %229 / 56367.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..79aa6af7b0 --- /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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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 +
40.7%40.7%
+ 		40.7 %229 / 56367.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..11cbf303e3 --- /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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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 +
40.7%40.7%
+ 		40.7 %229 / 56367.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..5d846259da --- /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:22956340.7 %
Date:2024-01-07 21:47:14Functions: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 +
40.7%40.7%
+ 		40.7 %229 / 56367.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..57b2864cc8 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html @@ -0,0 +1,520 @@ + + + + + + + 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:2142365658.6 %
Date:2024-01-07 21:47:14Functions:6911062.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)3
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<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::eland[abi:cxx11]()5
mrs_uav_managers::control_manager::ControlManager::elandSrv()5
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()7
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)8
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)12
mrs_uav_managers::control_manager::ControlManager::isOffboard()13
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)13
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)16
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)24
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)32
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)40
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)54
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::control_manager::ControlManager::initialize()57
mrs_uav_managers::control_manager::ControlManager::preinitialize()57
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)57
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)57
mrs_uav_managers::control_manager::ControlManager::onInit()57
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)70
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)82
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)84
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)84
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)96
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)111
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)114
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()124
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)131
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)134
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()146
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)182
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)227
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)285
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)291
mrs_uav_managers::control_manager::ControlManager::getMass()297
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)343
mrs_uav_managers::control_manager::ControlManager::ungripSrv()376
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)468
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)468
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)468
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)508
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)989
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)2856
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)2974
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7567
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)9545
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()9599
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()9601
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9713
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)15495
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)33506
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)53523
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)55326
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)64266
mrs_uav_managers::control_manager::ControlManager::updateTrackers()64638
mrs_uav_managers::control_manager::ControlManager::asyncControl()73885
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)74351
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)74351
mrs_uav_managers::control_manager::ControlManager::publish()74351
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)94453
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)173021
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)222997
+
+
+ + + +
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..26a569abc4 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html @@ -0,0 +1,520 @@ + + + + + + + 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:2142365658.6 %
Date:2024-01-07 21:47:14Functions:6911062.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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)343
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)15495
mrs_uav_managers::control_manager::ControlManager::initialize()57
mrs_uav_managers::control_manager::ControlManager::isOffboard()13
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)291
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)173021
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)9545
mrs_uav_managers::control_manager::ControlManager::asyncControl()73885
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)53523
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)24
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)84
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)40
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)70
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::preinitialize()57
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)134
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9713
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)33506
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)57
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()64638
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)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>)94453
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()9601
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)131
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)0
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)74351
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)8
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()146
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()9599
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)222997
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)16
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)54
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)12
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)468
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)114
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)2974
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)989
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)57
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)3
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)96
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)84
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()124
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)111
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)182
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)82
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)2856
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)32
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)508
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)74351
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)227
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)468
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)64266
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)468
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TrajectoryReferenceSrvResponse_<std::allocator<void> >&)3
mrs_uav_managers::control_manager::ControlManager::eland[abi:cxx11]()5
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)13
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::onInit()57
mrs_uav_managers::control_manager::ControlManager::getMass()297
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7567
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)55326
mrs_uav_managers::control_manager::ControlManager::publish()74351
mrs_uav_managers::control_manager::ControlManager::elandSrv()5
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()7
mrs_uav_managers::control_manager::ControlManager::ungripSrv()376
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)285
+
+
+ + + +
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..34b8182f12 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,8807 @@ + + + + + + + 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:2142365658.6 %
Date:2024-01-07 21:47:14Functions:6911062.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : #include <nodelet/nodelet.h>
+       6             : 
+       7             : #include <mrs_uav_managers/control_manager/common.h>
+       8             : #include <control_manager/output_publisher.h>
+       9             : 
+      10             : #include <mrs_uav_managers/controller.h>
+      11             : #include <mrs_uav_managers/tracker.h>
+      12             : 
+      13             : #include <mrs_msgs/String.h>
+      14             : #include <mrs_msgs/Float64Stamped.h>
+      15             : #include <mrs_msgs/ObstacleSectors.h>
+      16             : #include <mrs_msgs/BoolStamped.h>
+      17             : #include <mrs_msgs/BumperStatus.h>
+      18             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      19             : #include <mrs_msgs/DynamicsConstraints.h>
+      20             : #include <mrs_msgs/ControlError.h>
+      21             : #include <mrs_msgs/GetFloat64.h>
+      22             : #include <mrs_msgs/ValidateReference.h>
+      23             : #include <mrs_msgs/ValidateReferenceList.h>
+      24             : #include <mrs_msgs/BumperParamsSrv.h>
+      25             : #include <mrs_msgs/TrackerCommand.h>
+      26             : #include <mrs_msgs/EstimatorInput.h>
+      27             : 
+      28             : #include <geometry_msgs/Point32.h>
+      29             : #include <geometry_msgs/TwistStamped.h>
+      30             : #include <geometry_msgs/PoseArray.h>
+      31             : #include <geometry_msgs/Vector3Stamped.h>
+      32             : 
+      33             : #include <nav_msgs/Odometry.h>
+      34             : 
+      35             : #include <sensor_msgs/Joy.h>
+      36             : #include <sensor_msgs/NavSatFix.h>
+      37             : 
+      38             : #include <mrs_lib/safety_zone/safety_zone.h>
+      39             : #include <mrs_lib/profiler.h>
+      40             : #include <mrs_lib/param_loader.h>
+      41             : #include <mrs_lib/utils.h>
+      42             : #include <mrs_lib/mutex.h>
+      43             : #include <mrs_lib/transformer.h>
+      44             : #include <mrs_lib/geometry/misc.h>
+      45             : #include <mrs_lib/geometry/cyclic.h>
+      46             : #include <mrs_lib/attitude_converter.h>
+      47             : #include <mrs_lib/subscribe_handler.h>
+      48             : #include <mrs_lib/msg_extractor.h>
+      49             : #include <mrs_lib/quadratic_throttle_model.h>
+      50             : #include <mrs_lib/publisher_handler.h>
+      51             : #include <mrs_lib/service_client_handler.h>
+      52             : 
+      53             : #include <mrs_msgs/HwApiCapabilities.h>
+      54             : #include <mrs_msgs/HwApiStatus.h>
+      55             : #include <mrs_msgs/HwApiRcChannels.h>
+      56             : 
+      57             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      58             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      59             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      60             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      61             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      62             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      63             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      64             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      65             : #include <mrs_msgs/HwApiPositionCmd.h>
+      66             : 
+      67             : #include <std_msgs/Float64.h>
+      68             : 
+      69             : #include <future>
+      70             : 
+      71             : #include <pluginlib/class_loader.h>
+      72             : 
+      73             : #include <nodelet/loader.h>
+      74             : 
+      75             : #include <eigen3/Eigen/Eigen>
+      76             : 
+      77             : #include <visualization_msgs/Marker.h>
+      78             : #include <visualization_msgs/MarkerArray.h>
+      79             : 
+      80             : #include <mrs_msgs/Reference.h>
+      81             : #include <mrs_msgs/ReferenceStamped.h>
+      82             : #include <mrs_msgs/ReferenceList.h>
+      83             : #include <mrs_msgs/TrajectoryReference.h>
+      84             : 
+      85             : #include <mrs_msgs/ReferenceStampedSrv.h>
+      86             : #include <mrs_msgs/ReferenceStampedSrvRequest.h>
+      87             : #include <mrs_msgs/ReferenceStampedSrvResponse.h>
+      88             : 
+      89             : #include <mrs_msgs/VelocityReferenceStampedSrv.h>
+      90             : #include <mrs_msgs/VelocityReferenceStampedSrvRequest.h>
+      91             : #include <mrs_msgs/VelocityReferenceStampedSrvResponse.h>
+      92             : 
+      93             : #include <mrs_msgs/TransformReferenceSrv.h>
+      94             : #include <mrs_msgs/TransformReferenceSrvRequest.h>
+      95             : #include <mrs_msgs/TransformReferenceSrvResponse.h>
+      96             : 
+      97             : #include <mrs_msgs/TransformPoseSrv.h>
+      98             : #include <mrs_msgs/TransformPoseSrvRequest.h>
+      99             : #include <mrs_msgs/TransformPoseSrvResponse.h>
+     100             : 
+     101             : #include <mrs_msgs/TransformVector3Srv.h>
+     102             : #include <mrs_msgs/TransformVector3SrvRequest.h>
+     103             : #include <mrs_msgs/TransformVector3SrvResponse.h>
+     104             : 
+     105             : #include <mrs_msgs/Float64StampedSrv.h>
+     106             : #include <mrs_msgs/Float64StampedSrvRequest.h>
+     107             : #include <mrs_msgs/Float64StampedSrvResponse.h>
+     108             : 
+     109             : #include <mrs_msgs/Vec4.h>
+     110             : #include <mrs_msgs/Vec4Request.h>
+     111             : #include <mrs_msgs/Vec4Response.h>
+     112             : 
+     113             : #include <mrs_msgs/Vec1.h>
+     114             : #include <mrs_msgs/Vec1Request.h>
+     115             : #include <mrs_msgs/Vec1Response.h>
+     116             : 
+     117             : //}
+     118             : 
+     119             : /* defines //{ */
+     120             : 
+     121             : #define TAU 2 * M_PI
+     122             : #define REF_X 0
+     123             : #define REF_Y 1
+     124             : #define REF_Z 2
+     125             : #define REF_HEADING 3
+     126             : #define ELAND_STR "eland"
+     127             : #define EHOVER_STR "ehover"
+     128             : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+     129             : #define FAILSAFE_STR "failsafe"
+     130             : #define INPUT_UAV_STATE 0
+     131             : #define INPUT_ODOMETRY 1
+     132             : #define RC_DEADBAND 0.2
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* using //{ */
+     137             : 
+     138             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+     139             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+     140             : 
+     141             : using radians  = mrs_lib::geometry::radians;
+     142             : using sradians = mrs_lib::geometry::sradians;
+     143             : 
+     144             : //}
+     145             : 
+     146             : namespace mrs_uav_managers
+     147             : {
+     148             : 
+     149             : namespace control_manager
+     150             : {
+     151             : 
+     152             : /* //{ class ControlManager */
+     153             : 
+     154             : // state machine
+     155             : typedef enum
+     156             : {
+     157             : 
+     158             :   IDLE_STATE,
+     159             :   LANDING_STATE,
+     160             : 
+     161             : } LandingStates_t;
+     162             : 
+     163             : const char* state_names[2] = {
+     164             : 
+     165             :     "IDLING", "LANDING"};
+     166             : 
+     167             : // state machine
+     168             : typedef enum
+     169             : {
+     170             : 
+     171             :   FCU_FRAME,
+     172             :   RELATIVE_FRAME,
+     173             :   ABSOLUTE_FRAME
+     174             : 
+     175             : } ReferenceFrameType_t;
+     176             : 
+     177             : // state machine
+     178             : typedef enum
+     179             : {
+     180             : 
+     181             :   ESC_NONE_STATE     = 0,
+     182             :   ESC_EHOVER_STATE   = 1,
+     183             :   ESC_ELAND_STATE    = 2,
+     184             :   ESC_FAILSAFE_STATE = 3,
+     185             :   ESC_FINISHED_STATE = 4,
+     186             : 
+     187             : } EscalatingFailsafeStates_t;
+     188             : 
+     189             : /* class ControllerParams() //{ */
+     190             : 
+     191             : class ControllerParams {
+     192             : 
+     193             : public:
+     194             :   ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+     195             :                    bool human_switchable);
+     196             : 
+     197             : public:
+     198             :   double      failsafe_threshold;
+     199             :   double      eland_threshold;
+     200             :   double      odometry_innovation_threshold;
+     201             :   std::string address;
+     202             :   std::string name_space;
+     203             :   bool        human_switchable;
+     204             : };
+     205             : 
+     206         285 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     207         285 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     208             : 
+     209         285 :   this->eland_threshold               = eland_threshold;
+     210         285 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     211         285 :   this->failsafe_threshold            = failsafe_threshold;
+     212         285 :   this->address                       = address;
+     213         285 :   this->name_space                    = name_space;
+     214         285 :   this->human_switchable              = human_switchable;
+     215         285 : }
+     216             : 
+     217             : //}
+     218             : 
+     219             : /* class TrackerParams() //{ */
+     220             : 
+     221             : class TrackerParams {
+     222             : 
+     223             : public:
+     224             :   TrackerParams(std::string address, std::string name_space, bool human_switchable);
+     225             : 
+     226             : public:
+     227             :   std::string address;
+     228             :   std::string name_space;
+     229             :   bool        human_switchable;
+     230             : };
+     231             : 
+     232         343 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     233             : 
+     234         343 :   this->address          = address;
+     235         343 :   this->name_space       = name_space;
+     236         343 :   this->human_switchable = human_switchable;
+     237         343 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : class ControlManager : public nodelet::Nodelet {
+     242             : 
+     243             : public:
+     244             :   virtual void onInit();
+     245             : 
+     246             : private:
+     247             :   ros::NodeHandle   nh_;
+     248             :   std::atomic<bool> is_initialized_ = false;
+     249             :   std::string       _uav_name_;
+     250             :   std::string       _body_frame_;
+     251             : 
+     252             :   std::string _custom_config_;
+     253             :   std::string _platform_config_;
+     254             :   std::string _world_config_;
+     255             :   std::string _network_config_;
+     256             : 
+     257             :   // | --------------- dynamic loading of trackers -------------- |
+     258             : 
+     259             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     260             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     261             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     262             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     263             :   std::mutex                                                         mutex_tracker_list_;
+     264             : 
+     265             :   // | ------------- dynamic loading of controllers ------------- |
+     266             : 
+     267             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     268             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     269             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     270             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     271             :   std::mutex                                                            mutex_controller_list_;
+     272             : 
+     273             :   // | ------------------------- HW API ------------------------- |
+     274             : 
+     275             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     276             : 
+     277             :   OutputPublisher control_output_publisher_;
+     278             : 
+     279             :   ControlOutputModalities_t _hw_api_inputs_;
+     280             : 
+     281             :   double desired_uav_state_rate_ = 100.0;
+     282             : 
+     283             :   // this timer will check till we already got the hardware api diagnostics
+     284             :   // then it will trigger the initialization of the controllers and finish
+     285             :   // the initialization of the ControlManager
+     286             :   ros::Timer timer_hw_api_capabilities_;
+     287             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     288             : 
+     289             :   void preinitialize(void);
+     290             :   void initialize(void);
+     291             : 
+     292             :   // | ------------ tracker and controller switching ------------ |
+     293             : 
+     294             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     295             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     296             : 
+     297             :   // the time of last switching of a tracker or a controller
+     298             :   ros::Time  controller_tracker_switch_time_;
+     299             :   std::mutex mutex_controller_tracker_switch_time_;
+     300             : 
+     301             :   // | -------------------- the transformer  -------------------- |
+     302             : 
+     303             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     304             : 
+     305             :   // | ------------------- scope timer logger ------------------- |
+     306             : 
+     307             :   bool                                       scope_timer_enabled_ = false;
+     308             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     309             : 
+     310             :   // | --------------------- general params --------------------- |
+     311             : 
+     312             :   // defines the type of state input: odometry or uav_state mesasge types
+     313             :   int _state_input_;
+     314             : 
+     315             :   // names of important trackers
+     316             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     317             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     318             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     319             : 
+     320             :   // names of important controllers
+     321             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     322             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     323             : 
+     324             :   // joystick control
+     325             :   bool        _joystick_enabled_ = false;
+     326             :   int         _joystick_mode_;
+     327             :   std::string _joystick_tracker_name_;
+     328             :   std::string _joystick_controller_name_;
+     329             :   std::string _joystick_fallback_tracker_name_;
+     330             :   std::string _joystick_fallback_controller_name_;
+     331             : 
+     332             :   // should disarm after emergancy landing?
+     333             :   bool _eland_disarm_enabled_ = false;
+     334             : 
+     335             :   // enabling the emergency handoff -> will disable eland and failsafe
+     336             :   bool _rc_emergency_handoff_ = false;
+     337             : 
+     338             :   // what throttle should be output when null tracker is active?
+     339             :   double _min_throttle_null_tracker_ = 0.0;
+     340             : 
+     341             :   // rates of all the timers
+     342             :   double _status_timer_rate_   = 0;
+     343             :   double _safety_timer_rate_   = 0;
+     344             :   double _elanding_timer_rate_ = 0;
+     345             :   double _failsafe_timer_rate_ = 0;
+     346             :   double _bumper_timer_rate_   = 0;
+     347             : 
+     348             :   bool _snap_trajectory_to_safety_area_ = false;
+     349             : 
+     350             :   // | -------------- uav_state/odometry subscriber ------------- |
+     351             : 
+     352             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     353             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     354             : 
+     355             :   mrs_msgs::UavState uav_state_;
+     356             :   mrs_msgs::UavState previous_uav_state_;
+     357             :   bool               got_uav_state_               = false;
+     358             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     359             :   double             uav_roll_                    = 0;
+     360             :   double             uav_pitch_                   = 0;
+     361             :   double             uav_yaw_                     = 0;
+     362             :   double             uav_heading_                 = 0;
+     363             :   std::mutex         mutex_uav_state_;
+     364             : 
+     365             :   // odometry hiccup detection
+     366             :   double uav_state_avg_dt_        = 1;
+     367             :   double uav_state_hiccup_factor_ = 1;
+     368             :   int    uav_state_count_         = 0;
+     369             : 
+     370             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     371             : 
+     372             :   // | -------------- safety area max z subscriber -------------- |
+     373             : 
+     374             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     375             : 
+     376             :   // | ------------- odometry innovation subscriber ------------- |
+     377             : 
+     378             :   // odometry innovation is published by the odometry node
+     379             :   // it is used to issue eland if the estimator's input is too wonky
+     380             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     381             : 
+     382             :   // | --------------------- common handlers -------------------- |
+     383             : 
+     384             :   // contains handlers that are shared with trackers and controllers
+     385             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     386             : 
+     387             :   // | --------------- tracker and controller IDs --------------- |
+     388             : 
+     389             :   // keeping track of currently active controllers and trackers
+     390             :   unsigned int active_tracker_idx_    = 0;
+     391             :   unsigned int active_controller_idx_ = 0;
+     392             : 
+     393             :   // indeces of some notable trackers
+     394             :   unsigned int _ehover_tracker_idx_               = 0;
+     395             :   unsigned int _landoff_tracker_idx_              = 0;
+     396             :   unsigned int _joystick_tracker_idx_             = 0;
+     397             :   unsigned int _joystick_controller_idx_          = 0;
+     398             :   unsigned int _failsafe_controller_idx_          = 0;
+     399             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     400             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     401             :   unsigned int _null_tracker_idx_                 = 0;
+     402             :   unsigned int _eland_controller_idx_             = 0;
+     403             : 
+     404             :   // | -------------- enabling the output publisher ------------- |
+     405             : 
+     406             :   void              toggleOutput(const bool& input);
+     407             :   std::atomic<bool> output_enabled_ = false;
+     408             : 
+     409             :   // | ----------------------- publishers ----------------------- |
+     410             : 
+     411             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     413             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     414             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     415             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     416             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     417             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     418             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     419             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     420             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     421             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     422             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     423             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     424             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     425             :   mrs_lib::PublisherHandler<mrs_msgs::BumperStatus>              ph_bumper_status_;
+     426             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     427             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     428             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     429             : 
+     430             :   // | --------------------- service servers -------------------- |
+     431             : 
+     432             :   ros::ServiceServer service_server_switch_tracker_;
+     433             :   ros::ServiceServer service_server_switch_controller_;
+     434             :   ros::ServiceServer service_server_reset_tracker_;
+     435             :   ros::ServiceServer service_server_hover_;
+     436             :   ros::ServiceServer service_server_ehover_;
+     437             :   ros::ServiceServer service_server_failsafe_;
+     438             :   ros::ServiceServer service_server_failsafe_escalating_;
+     439             :   ros::ServiceServer service_server_toggle_output_;
+     440             :   ros::ServiceServer service_server_arm_;
+     441             :   ros::ServiceServer service_server_enable_callbacks_;
+     442             :   ros::ServiceServer service_server_set_constraints_;
+     443             :   ros::ServiceServer service_server_use_joystick_;
+     444             :   ros::ServiceServer service_server_use_safety_area_;
+     445             :   ros::ServiceServer service_server_emergency_reference_;
+     446             :   ros::ServiceServer service_server_pirouette_;
+     447             :   ros::ServiceServer service_server_eland_;
+     448             :   ros::ServiceServer service_server_parachute_;
+     449             : 
+     450             :   // human callbable services for references
+     451             :   ros::ServiceServer service_server_goto_;
+     452             :   ros::ServiceServer service_server_goto_fcu_;
+     453             :   ros::ServiceServer service_server_goto_relative_;
+     454             :   ros::ServiceServer service_server_goto_altitude_;
+     455             :   ros::ServiceServer service_server_set_heading_;
+     456             :   ros::ServiceServer service_server_set_heading_relative_;
+     457             : 
+     458             :   // the reference service and subscriber
+     459             :   ros::ServiceServer                                    service_server_reference_;
+     460             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     461             : 
+     462             :   // the velocity reference service and subscriber
+     463             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     464             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     465             : 
+     466             :   // trajectory tracking
+     467             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     468             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     469             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     470             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     471             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     472             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     473             : 
+     474             :   // transform service servers
+     475             :   ros::ServiceServer service_server_transform_reference_;
+     476             :   ros::ServiceServer service_server_transform_pose_;
+     477             :   ros::ServiceServer service_server_transform_vector3_;
+     478             : 
+     479             :   // safety area services
+     480             :   ros::ServiceServer service_server_validate_reference_;
+     481             :   ros::ServiceServer service_server_validate_reference_2d_;
+     482             :   ros::ServiceServer service_server_validate_reference_list_;
+     483             : 
+     484             :   // bumper service servers
+     485             :   ros::ServiceServer service_server_bumper_enabler_;
+     486             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     487             : 
+     488             :   // service clients
+     489             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     490             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     491             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     492             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     493             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     494             : 
+     495             :   // safety area min z servers
+     496             :   ros::ServiceServer service_server_set_min_z_;
+     497             :   ros::ServiceServer service_server_get_min_z_;
+     498             : 
+     499             :   // | --------- trackers' and controllers' last results -------- |
+     500             : 
+     501             :   // the last result of an active tracker
+     502             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     503             :   std::mutex                              mutex_last_tracker_cmd_;
+     504             : 
+     505             :   // the last result of an active controller
+     506             :   Controller::ControlOutput last_control_output_;
+     507             :   std::mutex                mutex_last_control_output_;
+     508             : 
+     509             :   // | -------------- HW API diagnostics subscriber ------------- |
+     510             : 
+     511             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     512             : 
+     513             :   bool offboard_mode_          = false;
+     514             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     515             :   bool armed_                  = false;
+     516             : 
+     517             :   // | -------------------- throttle and mass ------------------- |
+     518             : 
+     519             :   // throttle mass estimation during eland
+     520             :   double    throttle_mass_estimate_   = 0;
+     521             :   bool      throttle_under_threshold_ = false;
+     522             :   ros::Time throttle_mass_estimate_first_time_;
+     523             : 
+     524             :   // | ---------------------- safety params --------------------- |
+     525             : 
+     526             :   // failsafe when tilt error is too large
+     527             :   bool   _tilt_error_disarm_enabled_;
+     528             :   double _tilt_error_disarm_timeout_;
+     529             :   double _tilt_error_disarm_threshold_;
+     530             : 
+     531             :   ros::Time tilt_error_disarm_time_;
+     532             :   bool      tilt_error_disarm_over_thr_ = false;
+     533             : 
+     534             :   // elanding when tilt error is too large
+     535             :   bool   _tilt_limit_eland_enabled_;
+     536             :   double _tilt_limit_eland_ = 0;  // [rad]
+     537             : 
+     538             :   // disarming when tilt error is too large
+     539             :   bool   _tilt_limit_disarm_enabled_;
+     540             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     541             : 
+     542             :   // elanding when yaw error is too large
+     543             :   bool   _yaw_error_eland_enabled_;
+     544             :   double _yaw_error_eland_ = 0;  // [rad]
+     545             : 
+     546             :   // keeping track of control errors
+     547             :   std::optional<double> tilt_error_ = 0;
+     548             :   std::optional<double> yaw_error_  = 0;
+     549             :   std::mutex            mutex_attitude_error_;
+     550             : 
+     551             :   std::optional<Eigen::Vector3d> position_error_;
+     552             :   std::mutex                     mutex_position_error_;
+     553             : 
+     554             :   // control error for triggering failsafe, eland, etc.
+     555             :   // this filled with the current controllers failsafe threshold
+     556             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     557             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     558             :   bool   _odometry_innovation_check_enabled_ = false;
+     559             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     560             : 
+     561             :   bool callbacks_enabled_ = true;
+     562             : 
+     563             :   // | ------------------------ parachute ----------------------- |
+     564             : 
+     565             :   bool _parachute_enabled_ = false;
+     566             : 
+     567             :   std::tuple<bool, std::string> deployParachute(void);
+     568             :   bool                          parachuteSrv(void);
+     569             : 
+     570             :   // | ----------------------- safety area ---------------------- |
+     571             : 
+     572             :   // safety area
+     573             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     574             : 
+     575             :   std::atomic<bool> use_safety_area_ = false;
+     576             : 
+     577             :   std::string _safety_area_horizontal_frame_;
+     578             :   std::string _safety_area_vertical_frame_;
+     579             : 
+     580             :   double _safety_area_min_z_ = 0;
+     581             :   double _safety_area_max_z_ = 0;
+     582             : 
+     583             :   // safety area routines
+     584             :   // those are passed to trackers using the common_handlers object
+     585             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     586             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     587             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     588             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     589             :   double getMinZ(const std::string& frame_id);
+     590             :   double getMaxZ(const std::string& frame_id);
+     591             : 
+     592             :   // | ------------------------ callbacks ----------------------- |
+     593             : 
+     594             :   // topic callbacks
+     595             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     596             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     597             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     598             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     599             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     600             : 
+     601             :   // topic timeouts
+     602             :   void timeoutUavState(const double& missing_for);
+     603             : 
+     604             :   // switching controller and tracker services
+     605             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     606             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     607             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     608             : 
+     609             :   // reference callbacks
+     610             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     611             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     612             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     613             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     614             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     615             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     616             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     617             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     618             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     619             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     620             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     621             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     622             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     623             : 
+     624             :   // safety callbacks
+     625             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     626             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     627             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     628             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     634             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     635             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     636             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     637             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     638             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     639             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     640             :   bool callbackBumperSetParams(mrs_msgs::BumperParamsSrv::Request& req, mrs_msgs::BumperParamsSrv::Response& res);
+     641             : 
+     642             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     643             : 
+     644             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     645             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     646             :   bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+     647             : 
+     648             :   // transformation callbacks
+     649             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     650             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     651             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     652             : 
+     653             :   // | ----------------------- constraints ---------------------- |
+     654             : 
+     655             :   // sets constraints to all trackers
+     656             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     657             : 
+     658             :   // constraints management
+     659             :   bool              got_constraints_ = false;
+     660             :   std::mutex        mutex_constraints_;
+     661             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     662             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     663             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     664             :   std::atomic<bool> constraints_being_enforced_ = false;
+     665             : 
+     666             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     667             : 
+     668             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     669             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     670             : 
+     671             :   // | ------------------ emergency triggered? ------------------ |
+     672             : 
+     673             :   bool failsafe_triggered_ = false;
+     674             :   bool eland_triggered_    = false;
+     675             : 
+     676             :   // | ------------------------- timers ------------------------- |
+     677             : 
+     678             :   // timer for regular status publishing
+     679             :   ros::Timer timer_status_;
+     680             :   void       timerStatus(const ros::TimerEvent& event);
+     681             : 
+     682             :   // timer for issuing the failsafe landing
+     683             :   ros::Timer timer_failsafe_;
+     684             :   void       timerFailsafe(const ros::TimerEvent& event);
+     685             : 
+     686             :   // oneshot timer for running controllers and trackers
+     687             :   void              asyncControl(void);
+     688             :   std::atomic<bool> running_async_control_ = false;
+     689             :   std::future<void> async_control_result_;
+     690             : 
+     691             :   // timer for issuing emergancy landing
+     692             :   ros::Timer timer_eland_;
+     693             :   void       timerEland(const ros::TimerEvent& event);
+     694             : 
+     695             :   // timer for regular checking of controller errors
+     696             :   ros::Timer        timer_safety_;
+     697             :   void              timerSafety(const ros::TimerEvent& event);
+     698             :   std::atomic<bool> running_safety_timer_        = false;
+     699             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     700             : 
+     701             :   // timer for issuing the pirouette
+     702             :   ros::Timer timer_pirouette_;
+     703             :   void       timerPirouette(const ros::TimerEvent& event);
+     704             : 
+     705             :   // | --------------------- obstacle bumper -------------------- |
+     706             : 
+     707             :   // bumper timer
+     708             :   ros::Timer timer_bumper_;
+     709             :   void       timerBumper(const ros::TimerEvent& event);
+     710             : 
+     711             :   // bumper subscriber
+     712             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     713             : 
+     714             :   bool        _bumper_switch_tracker_    = false;
+     715             :   bool        _bumper_switch_controller_ = false;
+     716             :   std::string _bumper_tracker_name_;
+     717             :   std::string _bumper_controller_name_;
+     718             :   std::string bumper_previous_tracker_;
+     719             :   std::string bumper_previous_controller_;
+     720             : 
+     721             :   std::atomic<bool> bumper_enabled_ = false;
+     722             :   std::atomic<bool> repulsing_      = false;
+     723             : 
+     724             :   double _bumper_horizontal_distance_ = 0;
+     725             :   double _bumper_vertical_distance_   = 0;
+     726             : 
+     727             :   double _bumper_horizontal_overshoot_ = 0;
+     728             :   double _bumper_vertical_overshoot_   = 0;
+     729             : 
+     730             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     731             :   bool bumperPushFromObstacle(void);
+     732             : 
+     733             :   // | --------------- safety checks and failsafes -------------- |
+     734             : 
+     735             :   // escalating failsafe (eland -> failsafe -> disarm)
+     736             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     737             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     738             :   double                     _escalating_failsafe_timeout_         = 0;
+     739             :   ros::Time                  escalating_failsafe_time_;
+     740             :   bool                       _escalating_failsafe_ehover_   = false;
+     741             :   bool                       _escalating_failsafe_eland_    = false;
+     742             :   bool                       _escalating_failsafe_failsafe_ = false;
+     743             :   double                     _rc_escalating_failsafe_threshold_;
+     744             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     745             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     746             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     747             : 
+     748             :   std::string _tracker_error_action_;
+     749             : 
+     750             :   // emergancy landing state machine
+     751             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     752             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     753             :   std::mutex      mutex_landing_state_machine_;
+     754             :   void            changeLandingState(LandingStates_t new_state);
+     755             :   double          _uav_mass_ = 0;
+     756             :   double          _elanding_cutoff_mass_factor_;
+     757             :   double          _elanding_cutoff_timeout_;
+     758             :   double          landing_uav_mass_ = 0;
+     759             : 
+     760             :   // initial body disturbance loaded from params
+     761             :   double _initial_body_disturbance_x_ = 0;
+     762             :   double _initial_body_disturbance_y_ = 0;
+     763             : 
+     764             :   // profiling
+     765             :   mrs_lib::Profiler profiler_;
+     766             :   bool              _profiler_enabled_ = false;
+     767             : 
+     768             :   // diagnostics publishing
+     769             :   void       publishDiagnostics(void);
+     770             :   std::mutex mutex_diagnostics_;
+     771             : 
+     772             :   void                                             ungripSrv(void);
+     773             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     774             : 
+     775             :   bool isFlyingNormally(void);
+     776             : 
+     777             :   // | ------------------------ pirouette ----------------------- |
+     778             : 
+     779             :   bool       _pirouette_enabled_ = false;
+     780             :   double     _pirouette_speed_;
+     781             :   double     _pirouette_timer_rate_;
+     782             :   std::mutex mutex_pirouette_;
+     783             :   double     pirouette_initial_heading_;
+     784             :   double     pirouette_iterator_;
+     785             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     786             : 
+     787             :   // | -------------------- joystick control -------------------- |
+     788             : 
+     789             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     790             : 
+     791             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     792             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     793             : 
+     794             :   // joystick buttons mappings
+     795             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     796             : 
+     797             :   // channel numbers and channel multipliers
+     798             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     799             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     800             : 
+     801             :   ros::Timer timer_joystick_;
+     802             :   void       timerJoystick(const ros::TimerEvent& event);
+     803             :   double     _joystick_timer_rate_ = 0;
+     804             : 
+     805             :   double _joystick_carrot_distance_ = 0;
+     806             : 
+     807             :   ros::Time joystick_start_press_time_;
+     808             :   bool      joystick_start_pressed_ = false;
+     809             : 
+     810             :   ros::Time joystick_back_press_time_;
+     811             :   bool      joystick_back_pressed_ = false;
+     812             :   bool      joystick_goto_enabled_ = false;
+     813             : 
+     814             :   bool      joystick_failsafe_pressed_ = false;
+     815             :   ros::Time joystick_failsafe_press_time_;
+     816             : 
+     817             :   bool      joystick_eland_pressed_ = false;
+     818             :   ros::Time joystick_eland_press_time_;
+     819             : 
+     820             :   // | ------------------- RC joystick control ------------------ |
+     821             : 
+     822             :   // listening to the RC channels as told by pixhawk
+     823             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     824             : 
+     825             :   // the RC channel mapping of the main 4 control signals
+     826             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     827             : 
+     828             :   bool              _rc_goto_enabled_               = false;
+     829             :   std::atomic<bool> rc_goto_active_                 = false;
+     830             :   double            rc_joystick_channel_last_value_ = 0.5;
+     831             :   bool              rc_joystick_channel_was_low_    = false;
+     832             :   int               _rc_joystick_channel_           = 0;
+     833             : 
+     834             :   double _rc_horizontal_speed_ = 0;
+     835             :   double _rc_vertical_speed_   = 0;
+     836             :   double _rc_heading_rate_     = 0;
+     837             : 
+     838             :   // | ------------------- trajectory loading ------------------- |
+     839             : 
+     840             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     841             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     842             : 
+     843             :   // | --------------------- other routines --------------------- |
+     844             : 
+     845             :   // this is called to update the trackers and to receive position control command from the active one
+     846             :   void updateTrackers(void);
+     847             : 
+     848             :   // this is called to update the controllers and to receive attitude control command from the active one
+     849             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     850             : 
+     851             :   // sets the reference to the active tracker
+     852             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     853             : 
+     854             :   // sets the velocity reference to the active tracker
+     855             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     856             : 
+     857             :   // sets the reference trajectory to the active tracker
+     858             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     859             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     860             : 
+     861             :   // publishes
+     862             :   void publish(void);
+     863             : 
+     864             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     865             : 
+     866             :   double getMass(void);
+     867             : 
+     868             :   // publishes rviz-visualizable control reference
+     869             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     870             : 
+     871             :   void initializeControlOutput(void);
+     872             : 
+     873             :   // tell the mrs_odometry to disable its callbacks
+     874             :   void odometryCallbacksSrv(const bool input);
+     875             : 
+     876             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     877             : 
+     878             :   void                          setCallbacks(bool in);
+     879             :   bool                          isOffboard(void);
+     880             :   bool                          elandSrv(void);
+     881             :   std::tuple<bool, std::string> arming(const bool input);
+     882             : 
+     883             :   // safety functions impl
+     884             :   std::tuple<bool, std::string> ehover(void);
+     885             :   std::tuple<bool, std::string> hover(void);
+     886             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     887             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     888             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     889             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     890             :   std::tuple<bool, std::string> eland(void);
+     891             :   std::tuple<bool, std::string> failsafe(void);
+     892             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     893             : 
+     894             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     895             : };
+     896             : 
+     897             : //}
+     898             : 
+     899             : /* //{ onInit() */
+     900             : 
+     901          57 : void ControlManager::onInit() {
+     902          57 :   preinitialize();
+     903          57 : }
+     904             : 
+     905             : //}
+     906             : 
+     907             : /* preinitialize() //{ */
+     908             : 
+     909          57 : void ControlManager::preinitialize(void) {
+     910             : 
+     911          57 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     912             : 
+     913          57 :   ros::Time::waitForValid();
+     914             : 
+     915          57 :   mrs_lib::SubscribeHandlerOptions shopts;
+     916          57 :   shopts.nh                 = nh_;
+     917          57 :   shopts.node_name          = "ControlManager";
+     918          57 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     919          57 :   shopts.threadsafe         = true;
+     920          57 :   shopts.autostart          = true;
+     921          57 :   shopts.queue_size         = 10;
+     922          57 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     923             : 
+     924          57 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     925             : 
+     926          57 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     927          57 : }
+     928             : 
+     929             : //}
+     930             : 
+     931             : /* initialize() //{ */
+     932             : 
+     933          57 : void ControlManager::initialize(void) {
+     934             : 
+     935          57 :   joystick_start_press_time_      = ros::Time(0);
+     936          57 :   joystick_failsafe_press_time_   = ros::Time(0);
+     937          57 :   joystick_eland_press_time_      = ros::Time(0);
+     938          57 :   escalating_failsafe_time_       = ros::Time(0);
+     939          57 :   controller_tracker_switch_time_ = ros::Time(0);
+     940             : 
+     941          57 :   ROS_INFO("[ControlManager]: initializing");
+     942             : 
+     943             :   // --------------------------------------------------------------
+     944             :   // |         common handler for trackers and controllers        |
+     945             :   // --------------------------------------------------------------
+     946             : 
+     947          57 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     948             : 
+     949             :   // --------------------------------------------------------------
+     950             :   // |                           params                           |
+     951             :   // --------------------------------------------------------------
+     952             : 
+     953         114 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     954             : 
+     955          57 :   param_loader.loadParam("custom_config", _custom_config_);
+     956          57 :   param_loader.loadParam("platform_config", _platform_config_);
+     957          57 :   param_loader.loadParam("world_config", _world_config_);
+     958          57 :   param_loader.loadParam("network_config", _network_config_);
+     959             : 
+     960          57 :   if (_custom_config_ != "") {
+     961          57 :     param_loader.addYamlFile(_custom_config_);
+     962             :   }
+     963             : 
+     964          57 :   if (_platform_config_ != "") {
+     965          57 :     param_loader.addYamlFile(_platform_config_);
+     966             :   }
+     967             : 
+     968          57 :   if (_world_config_ != "") {
+     969          57 :     param_loader.addYamlFile(_world_config_);
+     970             :   }
+     971             : 
+     972          57 :   if (_network_config_ != "") {
+     973          57 :     param_loader.addYamlFile(_network_config_);
+     974             :   }
+     975             : 
+     976          57 :   param_loader.addYamlFileFromParam("private_config");
+     977          57 :   param_loader.addYamlFileFromParam("public_config");
+     978          57 :   param_loader.addYamlFileFromParam("private_trackers");
+     979          57 :   param_loader.addYamlFileFromParam("private_controllers");
+     980          57 :   param_loader.addYamlFileFromParam("public_controllers");
+     981             : 
+     982             :   // params passed from the launch file are not prefixed
+     983          57 :   param_loader.loadParam("uav_name", _uav_name_);
+     984          57 :   param_loader.loadParam("body_frame", _body_frame_);
+     985          57 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     986          57 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     987          57 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     988          57 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     989          57 :   param_loader.loadParam("g", common_handlers_->g);
+     990             : 
+     991             :   // motor params are also not prefixed, since they are common to more nodes
+     992          57 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+     993          57 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+     994          57 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+     995             : 
+     996             :   // | ----------------------- safety area ---------------------- |
+     997             : 
+     998             :   bool use_safety_area;
+     999          57 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1000          57 :   use_safety_area_ = use_safety_area;
+    1001             : 
+    1002          57 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1003             : 
+    1004          57 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1005          57 :   param_loader.loadParam("safety_area/vertical/min_z", _safety_area_min_z_);
+    1006          57 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1007             : 
+    1008          57 :   if (use_safety_area_) {
+    1009             : 
+    1010         156 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1011             : 
+    1012             :     try {
+    1013             : 
+    1014         104 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1015          52 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1016             : 
+    1017          52 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1018             :     }
+    1019             : 
+    1020           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1021           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1022           0 :       ros::shutdown();
+    1023             :     }
+    1024           0 :     catch (...) {
+    1025           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1026           0 :       ros::shutdown();
+    1027             :     }
+    1028             : 
+    1029          52 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1030             :   }
+    1031             : 
+    1032          57 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1033             : 
+    1034          57 :   param_loader.loadParam("state_input", _state_input_);
+    1035             : 
+    1036          57 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1037           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1038           0 :     ros::shutdown();
+    1039             :   }
+    1040             : 
+    1041          57 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1042          57 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1043          57 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1044             : 
+    1045          57 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1046          57 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1047          57 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1048          57 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1049          57 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1050             : 
+    1051          57 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1052          57 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1053          57 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1054          57 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1055          57 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1056          57 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1057          57 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1058          57 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1059             : 
+    1060          57 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1061          57 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1062             : 
+    1063          57 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1064             : 
+    1065          57 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1066           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1067           0 :     ros::shutdown();
+    1068             :   }
+    1069             : 
+    1070          57 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1071          57 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1072             : 
+    1073          57 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1074             : 
+    1075          57 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1076           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1077           0 :     ros::shutdown();
+    1078             :   }
+    1079             : 
+    1080          57 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1081          57 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1082             : 
+    1083          57 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1084             : 
+    1085          57 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1086           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1087           0 :     ros::shutdown();
+    1088             :   }
+    1089             : 
+    1090          57 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1091          57 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1092          57 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1093          57 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1094             : 
+    1095          57 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1096          57 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1097             : 
+    1098          57 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1099          57 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1100          57 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1101             : 
+    1102          57 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1103             : 
+    1104          57 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1105           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1106           0 :     ros::shutdown();
+    1107             :   }
+    1108             : 
+    1109             :   // default constraints
+    1110             : 
+    1111          57 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1112          57 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1113          57 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1114          57 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1115             : 
+    1116          57 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1117          57 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1118          57 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1119          57 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1120             : 
+    1121          57 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1122          57 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1123          57 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1124          57 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1125             : 
+    1126          57 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1127          57 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1128          57 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1129          57 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1130             : 
+    1131          57 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1132          57 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1133          57 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1134             : 
+    1135          57 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1136             : 
+    1137          57 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1138             : 
+    1139             :   // joystick
+    1140             : 
+    1141          57 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1142          57 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1143          57 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1144          57 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1145          57 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1146          57 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1147          57 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1148          57 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1149             : 
+    1150          57 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1151          57 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1152          57 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1153          57 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1154          57 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1155          57 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1156          57 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1157          57 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1158          57 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1159          57 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1160             : 
+    1161             :   // load channels
+    1162          57 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1163          57 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1164          57 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1165          57 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1166             : 
+    1167             :   // load channel multipliers
+    1168          57 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1169          57 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1170          57 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1171          57 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1172             : 
+    1173             :   bool bumper_enabled;
+    1174          57 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1175          57 :   bumper_enabled_ = bumper_enabled;
+    1176             : 
+    1177          57 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1178          57 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1179          57 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1180          57 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1181          57 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1182             : 
+    1183          57 :   param_loader.loadParam("obstacle_bumper/horizontal/threshold_distance", _bumper_horizontal_distance_);
+    1184          57 :   param_loader.loadParam("obstacle_bumper/vertical/threshold_distance", _bumper_vertical_distance_);
+    1185             : 
+    1186          57 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1187          57 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1188             : 
+    1189          57 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1190             : 
+    1191          57 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1192             : 
+    1193             :   // check the values of tracker error action
+    1194          57 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1195           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1196             :               EHOVER_STR);
+    1197           0 :     ros::shutdown();
+    1198             :   }
+    1199             : 
+    1200          57 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1201          57 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1202          57 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1203          57 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1204          57 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1205             : 
+    1206          57 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1207          57 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1208          57 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1209          57 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1210             : 
+    1211          57 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1212          57 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1213             : 
+    1214          57 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1215             : 
+    1216             :   // --------------------------------------------------------------
+    1217             :   // |             initialize the last control output             |
+    1218             :   // --------------------------------------------------------------
+    1219             : 
+    1220          57 :   initializeControlOutput();
+    1221             : 
+    1222             :   // | --------------------- tf transformer --------------------- |
+    1223             : 
+    1224          57 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1225          57 :   transformer_->setDefaultPrefix(_uav_name_);
+    1226          57 :   transformer_->retryLookupNewest(true);
+    1227             : 
+    1228             :   // | ------------------- scope timer logger ------------------- |
+    1229             : 
+    1230          57 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1231         171 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1232          57 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1233             : 
+    1234             :   // bind transformer to trackers and controllers for use
+    1235          57 :   common_handlers_->transformer = transformer_;
+    1236             : 
+    1237             :   // bind scope timer to trackers and controllers for use
+    1238          57 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1239          57 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1240             : 
+    1241          57 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1242          57 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1243          57 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1244          57 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1245          57 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1246             : 
+    1247          57 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1248             : 
+    1249          57 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1250             : 
+    1251          57 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1252             : 
+    1253          57 :   common_handlers_->uav_name = _uav_name_;
+    1254             : 
+    1255          57 :   common_handlers_->parent_nh = nh_;
+    1256             : 
+    1257             :   // --------------------------------------------------------------
+    1258             :   // |                        load trackers                       |
+    1259             :   // --------------------------------------------------------------
+    1260             : 
+    1261         114 :   std::vector<std::string> custom_trackers;
+    1262             : 
+    1263          57 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1264          57 :   param_loader.loadParam("trackers", custom_trackers);
+    1265             : 
+    1266          57 :   if (!custom_trackers.empty()) {
+    1267           1 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1268             :   }
+    1269             : 
+    1270          57 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1271          57 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1272             : 
+    1273          57 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1274             : 
+    1275         400 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1276             : 
+    1277         686 :     std::string tracker_name = _tracker_names_[i];
+    1278             : 
+    1279             :     // load the controller parameters
+    1280         686 :     std::string address;
+    1281         686 :     std::string name_space;
+    1282             :     bool        human_switchable;
+    1283             : 
+    1284         343 :     param_loader.loadParam(tracker_name + "/address", address);
+    1285         343 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1286         343 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1287             : 
+    1288        1029 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1289         343 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1290             : 
+    1291             :     try {
+    1292         343 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1293         343 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1294             :     }
+    1295           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1296           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1297           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1298           0 :       ros::shutdown();
+    1299             :     }
+    1300           0 :     catch (pluginlib::PluginlibException& ex) {
+    1301           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1302           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1303           0 :       ros::shutdown();
+    1304             :     }
+    1305             :   }
+    1306             : 
+    1307          57 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1308             : 
+    1309         400 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1310             : 
+    1311         343 :     std::map<std::string, TrackerParams>::iterator it;
+    1312         343 :     it = trackers_.find(_tracker_names_[i]);
+    1313             : 
+    1314             :     // create private handlers
+    1315             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1316         686 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1317             : 
+    1318         343 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1319         343 :     private_handlers->name_space     = it->second.name_space;
+    1320         343 :     private_handlers->runtime_name   = _tracker_names_[i];
+    1321         343 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_[i]);
+    1322             : 
+    1323         343 :     if (_custom_config_ != "") {
+    1324         343 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1325             :     }
+    1326             : 
+    1327         343 :     if (_platform_config_ != "") {
+    1328         343 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1329             :     }
+    1330             : 
+    1331         343 :     if (_world_config_ != "") {
+    1332         343 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1333             :     }
+    1334             : 
+    1335         343 :     if (_network_config_ != "") {
+    1336         343 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1337             :     }
+    1338             : 
+    1339         343 :     bool success = false;
+    1340             : 
+    1341             :     try {
+    1342         343 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1343         343 :       success = tracker_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1344             :     }
+    1345           0 :     catch (std::runtime_error& ex) {
+    1346           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1347             :     }
+    1348             : 
+    1349         343 :     if (!success) {
+    1350           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1351           0 :       ros::shutdown();
+    1352             :     }
+    1353             :   }
+    1354             : 
+    1355          57 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1356             : 
+    1357             :   // --------------------------------------------------------------
+    1358             :   // |           check the existance of selected trackers         |
+    1359             :   // --------------------------------------------------------------
+    1360             : 
+    1361             :   // | ------ check for the existance of the hover tracker ------ |
+    1362             : 
+    1363             :   // check if the hover_tracker is within the loaded trackers
+    1364             :   {
+    1365          57 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1366             : 
+    1367          57 :     if (idx) {
+    1368          57 :       _ehover_tracker_idx_ = idx.value();
+    1369             :     } else {
+    1370           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1371           0 :       ros::shutdown();
+    1372             :     }
+    1373             :   }
+    1374             : 
+    1375             :   // | ----- check for the existence of the landoff tracker ----- |
+    1376             : 
+    1377             :   {
+    1378          57 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1379             : 
+    1380          57 :     if (idx) {
+    1381          57 :       _landoff_tracker_idx_ = idx.value();
+    1382             :     } else {
+    1383           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1384           0 :       ros::shutdown();
+    1385             :     }
+    1386             :   }
+    1387             : 
+    1388             :   // | ------- check for the existence of the null tracker ------ |
+    1389             : 
+    1390             :   {
+    1391          57 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1392             : 
+    1393          57 :     if (idx) {
+    1394          57 :       _null_tracker_idx_ = idx.value();
+    1395             :     } else {
+    1396           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1397           0 :       ros::shutdown();
+    1398             :     }
+    1399             :   }
+    1400             : 
+    1401             :   // --------------------------------------------------------------
+    1402             :   // |         check existance of trackers for joystick           |
+    1403             :   // --------------------------------------------------------------
+    1404             : 
+    1405          57 :   if (_joystick_enabled_) {
+    1406             : 
+    1407          57 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1408             : 
+    1409          57 :     if (idx) {
+    1410          57 :       _joystick_tracker_idx_ = idx.value();
+    1411             :     } else {
+    1412           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1413           0 :       ros::shutdown();
+    1414             :     }
+    1415             :   }
+    1416             : 
+    1417          57 :   if (_bumper_switch_tracker_) {
+    1418             : 
+    1419          57 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1420             : 
+    1421          57 :     if (!idx) {
+    1422           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1423           0 :       ros::shutdown();
+    1424             :     }
+    1425             :   }
+    1426             : 
+    1427             :   {
+    1428          57 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1429             : 
+    1430          57 :     if (idx) {
+    1431          57 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1432             :     } else {
+    1433           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1434           0 :       ros::shutdown();
+    1435             :     }
+    1436             :   }
+    1437             : 
+    1438             :   // --------------------------------------------------------------
+    1439             :   // |                    load the controllers                    |
+    1440             :   // --------------------------------------------------------------
+    1441             : 
+    1442         114 :   std::vector<std::string> custom_controllers;
+    1443             : 
+    1444          57 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1445          57 :   param_loader.loadParam("controllers", custom_controllers);
+    1446             : 
+    1447          57 :   if (!custom_controllers.empty()) {
+    1448           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1449             :   }
+    1450             : 
+    1451          57 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1452             : 
+    1453             :   // for each controller in the list
+    1454         342 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1455             : 
+    1456         570 :     std::string controller_name = _controller_names_[i];
+    1457             : 
+    1458             :     // load the controller parameters
+    1459         570 :     std::string address;
+    1460         570 :     std::string name_space;
+    1461             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1462             :     bool        human_switchable;
+    1463         285 :     param_loader.loadParam(controller_name + "/address", address);
+    1464         285 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1465         285 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1466         285 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1467         285 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1468         285 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1469             : 
+    1470             :     // check if the controller can output some of the required outputs
+    1471             :     {
+    1472             : 
+    1473         285 :       ControlOutputModalities_t outputs;
+    1474         285 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1475         285 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1476         285 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1477         285 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1478         285 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1479         285 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1480         285 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1481         285 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1482         285 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1483             : 
+    1484         285 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1485         285 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1486         285 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1487         285 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1488         285 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1489         285 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1490         285 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1491         285 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1492         285 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1493             : 
+    1494         270 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1495         555 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1496             : 
+    1497         285 :       if (!meets_requirements) {
+    1498             : 
+    1499           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1500             :                   controller_name.c_str());
+    1501             : 
+    1502           0 :         if (_hw_api_inputs_.actuators) {
+    1503           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1504             :         }
+    1505             : 
+    1506           0 :         if (_hw_api_inputs_.control_group) {
+    1507           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1508             :         }
+    1509             : 
+    1510           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1511           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1512             :         }
+    1513             : 
+    1514           0 :         if (_hw_api_inputs_.attitude) {
+    1515           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1516             :         }
+    1517             : 
+    1518           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1519           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1520             :         }
+    1521             : 
+    1522           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1523           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1524             :         }
+    1525             : 
+    1526           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1527           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1528             :         }
+    1529             : 
+    1530           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1531           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1532             :         }
+    1533             : 
+    1534           0 :         if (_hw_api_inputs_.position) {
+    1535           0 :           ROS_ERROR("[ControlManager]: - position");
+    1536             :         }
+    1537             : 
+    1538           0 :         ros::shutdown();
+    1539             :       }
+    1540             : 
+    1541         285 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1542           0 :         ROS_ERROR(
+    1543             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1544           0 :         ros::shutdown();
+    1545             :       }
+    1546             :     }
+    1547             : 
+    1548             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1549             : 
+    1550         285 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1551             : 
+    1552         285 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1553          30 :       _safety_timer_rate_     = 200.0;
+    1554          30 :       desired_uav_state_rate_ = 250.0;
+    1555         255 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1556         205 :       _safety_timer_rate_     = 100.0;
+    1557         205 :       desired_uav_state_rate_ = 100.0;
+    1558          50 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1559          20 :       _safety_timer_rate_     = 30.0;
+    1560          20 :       _status_timer_rate_     = 1.0;
+    1561          20 :       desired_uav_state_rate_ = 40.0;
+    1562             : 
+    1563          20 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1564           4 :         _uav_state_max_missing_time_ = 0.2;
+    1565             :       }
+    1566          30 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1567          30 :       _safety_timer_rate_     = 20.0;
+    1568          30 :       _status_timer_rate_     = 1.0;
+    1569          30 :       desired_uav_state_rate_ = 20.0;
+    1570             : 
+    1571          30 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1572           6 :         _uav_state_max_missing_time_ = 1.0;
+    1573             :       }
+    1574             :     }
+    1575             : 
+    1576         285 :     if (eland_threshold == 0) {
+    1577          58 :       eland_threshold = 1e6;
+    1578             :     }
+    1579             : 
+    1580         285 :     if (failsafe_threshold == 0) {
+    1581          58 :       failsafe_threshold = 1e6;
+    1582             :     }
+    1583             : 
+    1584         285 :     if (odometry_innovation_threshold == 0) {
+    1585          59 :       odometry_innovation_threshold = 1e6;
+    1586             :     }
+    1587             : 
+    1588         855 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1589         285 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1590             : 
+    1591             :     try {
+    1592         285 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1593         285 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1594             :     }
+    1595           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1596           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1597           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1598           0 :       ros::shutdown();
+    1599             :     }
+    1600           0 :     catch (pluginlib::PluginlibException& ex) {
+    1601           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1602           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1603           0 :       ros::shutdown();
+    1604             :     }
+    1605             :   }
+    1606             : 
+    1607          57 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1608             : 
+    1609         342 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1610             : 
+    1611         285 :     std::map<std::string, ControllerParams>::iterator it;
+    1612         285 :     it = controllers_.find(_controller_names_[i]);
+    1613             : 
+    1614             :     // create private handlers
+    1615             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1616         570 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1617             : 
+    1618         285 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1619         285 :     private_handlers->name_space     = it->second.name_space;
+    1620         285 :     private_handlers->runtime_name   = _controller_names_[i];
+    1621         285 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_[i]);
+    1622             : 
+    1623         285 :     if (_custom_config_ != "") {
+    1624         285 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1625             :     }
+    1626             : 
+    1627         285 :     if (_platform_config_ != "") {
+    1628         285 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1629             :     }
+    1630             : 
+    1631         285 :     if (_world_config_ != "") {
+    1632         285 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1633             :     }
+    1634             : 
+    1635         285 :     if (_network_config_ != "") {
+    1636         285 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1637             :     }
+    1638             : 
+    1639         285 :     bool success = false;
+    1640             : 
+    1641             :     try {
+    1642             : 
+    1643         285 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1644         285 :       success = controller_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1645             :     }
+    1646           0 :     catch (std::runtime_error& ex) {
+    1647           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1648             :     }
+    1649             : 
+    1650         285 :     if (!success) {
+    1651           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1652           0 :       ros::shutdown();
+    1653             :     }
+    1654             :   }
+    1655             : 
+    1656          57 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1657             : 
+    1658             :   {
+    1659          57 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1660             : 
+    1661          57 :     if (idx) {
+    1662          57 :       _failsafe_controller_idx_ = idx.value();
+    1663             :     } else {
+    1664           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1665           0 :       ros::shutdown();
+    1666             :     }
+    1667             :   }
+    1668             : 
+    1669             :   {
+    1670          57 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1671             : 
+    1672          57 :     if (idx) {
+    1673          57 :       _eland_controller_idx_ = idx.value();
+    1674             :     } else {
+    1675           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1676           0 :       ros::shutdown();
+    1677             :     }
+    1678             :   }
+    1679             : 
+    1680             :   {
+    1681          57 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1682             : 
+    1683          57 :     if (idx) {
+    1684          57 :       _joystick_controller_idx_ = idx.value();
+    1685             :     } else {
+    1686           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1687           0 :       ros::shutdown();
+    1688             :     }
+    1689             :   }
+    1690             : 
+    1691          57 :   if (_bumper_switch_controller_) {
+    1692             : 
+    1693          57 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1694             : 
+    1695          57 :     if (!idx) {
+    1696           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1697           0 :       ros::shutdown();
+    1698             :     }
+    1699             :   }
+    1700             : 
+    1701             :   {
+    1702          57 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1703             : 
+    1704          57 :     if (idx) {
+    1705          57 :       _joystick_fallback_controller_idx_ = idx.value();
+    1706             :     } else {
+    1707           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1708           0 :       ros::shutdown();
+    1709             :     }
+    1710             :   }
+    1711             : 
+    1712             :   // --------------------------------------------------------------
+    1713             :   // |                  activate the NullTracker                  |
+    1714             :   // --------------------------------------------------------------
+    1715             : 
+    1716          57 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1717             : 
+    1718          57 :   tracker_list_[_null_tracker_idx_]->activate(last_tracker_cmd_);
+    1719          57 :   active_tracker_idx_ = _null_tracker_idx_;
+    1720             : 
+    1721             :   // --------------------------------------------------------------
+    1722             :   // |    activate the eland controller as the first controller   |
+    1723             :   // --------------------------------------------------------------
+    1724             : 
+    1725          57 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_[_eland_controller_idx_].c_str());
+    1726             : 
+    1727          57 :   controller_list_[_eland_controller_idx_]->activate(last_control_output_);
+    1728          57 :   active_controller_idx_ = _eland_controller_idx_;
+    1729             : 
+    1730             :   // update the time
+    1731             :   {
+    1732         114 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1733             : 
+    1734          57 :     controller_tracker_switch_time_ = ros::Time::now();
+    1735             :   }
+    1736             : 
+    1737          57 :   output_enabled_ = false;
+    1738             : 
+    1739             :   // | --------------- set the default constraints -------------- |
+    1740             : 
+    1741          57 :   sanitized_constraints_ = current_constraints_;
+    1742          57 :   setConstraints(current_constraints_);
+    1743             : 
+    1744             :   // | ------------------------ profiler ------------------------ |
+    1745             : 
+    1746          57 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1747             : 
+    1748             :   // | ----------------------- publishers ----------------------- |
+    1749             : 
+    1750          57 :   control_output_publisher_ = OutputPublisher(nh_);
+    1751             : 
+    1752          57 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1753          57 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1754          57 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1755          57 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1756          57 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1757          57 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1758          57 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1759          57 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1760          57 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1761          57 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1762          57 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1763          57 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1764          57 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1765          57 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1766          57 :   ph_bumper_status_                      = mrs_lib::PublisherHandler<mrs_msgs::BumperStatus>(nh_, "bumper_status_out", 1);
+    1767          57 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1768          57 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1769          57 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1770          57 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1771          57 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1772             : 
+    1773             :   // | ----------------------- subscribers ---------------------- |
+    1774             : 
+    1775         114 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1776          57 :   shopts.nh                 = nh_;
+    1777          57 :   shopts.node_name          = "ControlManager";
+    1778          57 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1779          57 :   shopts.threadsafe         = true;
+    1780          57 :   shopts.autostart          = true;
+    1781          57 :   shopts.queue_size         = 10;
+    1782          57 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1783             : 
+    1784          57 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1785          57 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1786           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1787           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1788             :   }
+    1789             : 
+    1790          57 :   if (_odometry_innovation_check_enabled_) {
+    1791          57 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1792             :   }
+    1793             : 
+    1794          57 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1795          57 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1796          57 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1797          57 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1798          57 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1799             : 
+    1800          57 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1801             : 
+    1802             :   // | -------------------- general services -------------------- |
+    1803             : 
+    1804          57 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1805          57 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1806          57 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1807          57 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1808          57 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1809          57 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1810          57 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1811          57 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1812          57 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1813          57 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1814          57 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1815          57 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1816          57 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1817          57 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1818          57 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1819          57 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1820          57 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1821          57 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1822          57 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1823          57 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1824          57 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1825          57 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1826          57 :   service_server_validate_reference_list_    = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+    1827          57 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1828          57 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1829          57 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1830          57 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1831             : 
+    1832          57 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1833          57 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1834          57 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1835          57 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1836          57 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1837          57 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1838             : 
+    1839             :   // | ---------------- setpoint command services --------------- |
+    1840             : 
+    1841             :   // human callable
+    1842          57 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1843          57 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1844          57 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1845          57 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1846          57 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1847          57 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1848             : 
+    1849          57 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1850          57 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1851             : 
+    1852          57 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1853             :   sh_velocity_reference_ =
+    1854          57 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1855             : 
+    1856          57 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1857             :   sh_trajectory_reference_ =
+    1858          57 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1859             : 
+    1860             :   // | --------------------- other services --------------------- |
+    1861             : 
+    1862          57 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1863          57 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1864             : 
+    1865             :   // | ------------------------- timers ------------------------- |
+    1866             : 
+    1867          57 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1868          57 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1869          57 :   timer_bumper_    = nh_.createTimer(ros::Rate(_bumper_timer_rate_), &ControlManager::timerBumper, this, false, bumper_enabled_);
+    1870          57 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1871          57 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1872          57 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1873          57 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1874             : 
+    1875             :   // | ----------------------- finish init ---------------------- |
+    1876             : 
+    1877          57 :   if (!param_loader.loadedSuccessfully()) {
+    1878           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1879           0 :     ros::shutdown();
+    1880             :   }
+    1881             : 
+    1882          57 :   is_initialized_ = true;
+    1883             : 
+    1884          57 :   ROS_INFO("[ControlManager]: initialized");
+    1885          57 : }
+    1886             : 
+    1887             : //}
+    1888             : 
+    1889             : // --------------------------------------------------------------
+    1890             : // |                           timers                           |
+    1891             : // --------------------------------------------------------------
+    1892             : 
+    1893             : /* timerHwApiCapabilities() //{ */
+    1894             : 
+    1895          96 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1896             : 
+    1897         192 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1898         192 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1899             : 
+    1900          96 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1901          39 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1902          39 :     return;
+    1903             :   }
+    1904             : 
+    1905         114 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1906             : 
+    1907          57 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1908             : 
+    1909          57 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1910           3 :     ROS_INFO("[ControlManager]: - actuator command");
+    1911           3 :     _hw_api_inputs_.actuators = true;
+    1912             :   }
+    1913             : 
+    1914          57 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1915           3 :     ROS_INFO("[ControlManager]: - control group command");
+    1916           3 :     _hw_api_inputs_.control_group = true;
+    1917             :   }
+    1918             : 
+    1919          57 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1920          38 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1921          38 :     _hw_api_inputs_.attitude_rate = true;
+    1922             :   }
+    1923             : 
+    1924          57 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1925          38 :     ROS_INFO("[ControlManager]: - attitude command");
+    1926          38 :     _hw_api_inputs_.attitude = true;
+    1927             :   }
+    1928             : 
+    1929          57 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1930           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1931           2 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1932             :   }
+    1933             : 
+    1934          57 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1935           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1936           2 :     _hw_api_inputs_.acceleration_hdg = true;
+    1937             :   }
+    1938             : 
+    1939          57 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1940           2 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1941           2 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1942             :   }
+    1943             : 
+    1944          57 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1945           2 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1946           2 :     _hw_api_inputs_.velocity_hdg = true;
+    1947             :   }
+    1948             : 
+    1949          57 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1950           2 :     ROS_INFO("[ControlManager]: - position command");
+    1951           2 :     _hw_api_inputs_.position = true;
+    1952             :   }
+    1953             : 
+    1954          57 :   initialize();
+    1955             : 
+    1956          57 :   timer_hw_api_capabilities_.stop();
+    1957             : }
+    1958             : 
+    1959             : //}
+    1960             : 
+    1961             : /* //{ timerStatus() */
+    1962             : 
+    1963        9545 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1964             : 
+    1965        9545 :   if (!is_initialized_)
+    1966           0 :     return;
+    1967             : 
+    1968       28635 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1969       28635 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1970             : 
+    1971             :   // copy member variables
+    1972       19090 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1973       19090 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1974       19090 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    1975        9545 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    1976        9545 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    1977        9545 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    1978        9545 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    1979             : 
+    1980             :   double uav_x, uav_y, uav_z;
+    1981        9545 :   uav_x = uav_state.pose.position.x;
+    1982        9545 :   uav_y = uav_state.pose.position.y;
+    1983        9545 :   uav_z = uav_state.pose.position.z;
+    1984             : 
+    1985             :   // --------------------------------------------------------------
+    1986             :   // |                      print the status                      |
+    1987             :   // --------------------------------------------------------------
+    1988             : 
+    1989             :   {
+    1990       19090 :     std::string controller = _controller_names_[active_controller_idx];
+    1991       19090 :     std::string tracker    = _tracker_names_[active_tracker_idx];
+    1992        9545 :     double      mass       = last_control_output.diagnostics.total_mass;
+    1993        9545 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    1994        9545 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    1995        9545 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    1996        9545 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    1997             : 
+    1998        9545 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    1999             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2000             :   }
+    2001             : 
+    2002             :   // --------------------------------------------------------------
+    2003             :   // |                   publish the diagnostics                  |
+    2004             :   // --------------------------------------------------------------
+    2005             : 
+    2006        9545 :   publishDiagnostics();
+    2007             : 
+    2008             :   // --------------------------------------------------------------
+    2009             :   // |                publish if the offboard is on               |
+    2010             :   // --------------------------------------------------------------
+    2011             : 
+    2012        9545 :   if (offboard_mode_) {
+    2013             : 
+    2014        7115 :     std_msgs::Empty offboard_on_out;
+    2015             : 
+    2016        7115 :     ph_offboard_on_.publish(offboard_on_out);
+    2017             :   }
+    2018             : 
+    2019             :   // --------------------------------------------------------------
+    2020             :   // |                   publish the tilt error                   |
+    2021             :   // --------------------------------------------------------------
+    2022             :   {
+    2023       19090 :     std::scoped_lock lock(mutex_attitude_error_);
+    2024             : 
+    2025        9545 :     if (tilt_error_) {
+    2026             : 
+    2027       19090 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2028        9545 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2029        9545 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2030        9545 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2031             : 
+    2032        9545 :       ph_tilt_error_.publish(tilt_error_out);
+    2033             :     }
+    2034             :   }
+    2035             : 
+    2036             :   // --------------------------------------------------------------
+    2037             :   // |                  publish the control error                 |
+    2038             :   // --------------------------------------------------------------
+    2039             : 
+    2040        9545 :   if (position_error) {
+    2041             : 
+    2042        6601 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2043             : 
+    2044       13202 :     mrs_msgs::ControlError msg_out;
+    2045             : 
+    2046        6601 :     msg_out.header.stamp    = ros::Time::now();
+    2047        6601 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2048             : 
+    2049        6601 :     msg_out.position_errors.x    = pos_error_value[0];
+    2050        6601 :     msg_out.position_errors.y    = pos_error_value[1];
+    2051        6601 :     msg_out.position_errors.z    = pos_error_value[2];
+    2052        6601 :     msg_out.total_position_error = pos_error_value.norm();
+    2053             : 
+    2054        6601 :     if (yaw_error_) {
+    2055        6601 :       msg_out.yaw_error = yaw_error.value();
+    2056             :     }
+    2057             : 
+    2058        6601 :     std::map<std::string, ControllerParams>::iterator it;
+    2059             : 
+    2060        6601 :     it = controllers_.find(_controller_names_[active_controller_idx]);
+    2061             : 
+    2062        6601 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2063        6601 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2064             : 
+    2065        6601 :     ph_control_error_.publish(msg_out);
+    2066             :   }
+    2067             : 
+    2068             :   // --------------------------------------------------------------
+    2069             :   // |                  publish the mass estimate                 |
+    2070             :   // --------------------------------------------------------------
+    2071             : 
+    2072        9545 :   if (last_control_output.diagnostics.mass_estimator) {
+    2073             : 
+    2074        5477 :     std_msgs::Float64 mass_estimate_out;
+    2075        5477 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2076             : 
+    2077        5477 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2078             :   }
+    2079             : 
+    2080             :   // --------------------------------------------------------------
+    2081             :   // |                 publish the current heading                |
+    2082             :   // --------------------------------------------------------------
+    2083             : 
+    2084        9545 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2085             : 
+    2086             :     try {
+    2087             : 
+    2088             :       double heading;
+    2089             : 
+    2090        7943 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2091             : 
+    2092       15886 :       mrs_msgs::Float64Stamped heading_out;
+    2093        7943 :       heading_out.header = uav_state.header;
+    2094        7943 :       heading_out.value  = heading;
+    2095             : 
+    2096        7943 :       ph_heading_.publish(heading_out);
+    2097             :     }
+    2098           0 :     catch (...) {
+    2099           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2100             :     }
+    2101             :   }
+    2102             : 
+    2103             :   // --------------------------------------------------------------
+    2104             :   // |                  publish the current speed                 |
+    2105             :   // --------------------------------------------------------------
+    2106             : 
+    2107        9545 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2108             : 
+    2109        7943 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2110             : 
+    2111       15886 :     mrs_msgs::Float64Stamped speed_out;
+    2112        7943 :     speed_out.header = uav_state.header;
+    2113        7943 :     speed_out.value  = speed;
+    2114             : 
+    2115        7943 :     ph_speed_.publish(speed_out);
+    2116             :   }
+    2117             : 
+    2118             :   // --------------------------------------------------------------
+    2119             :   // |               publish the safety area markers              |
+    2120             :   // --------------------------------------------------------------
+    2121             : 
+    2122        9545 :   if (use_safety_area_) {
+    2123             : 
+    2124       17054 :     mrs_msgs::ReferenceStamped temp_ref;
+    2125        8527 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2126             : 
+    2127       17054 :     geometry_msgs::TransformStamped tf;
+    2128             : 
+    2129       25581 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2130             : 
+    2131        8527 :     if (ret) {
+    2132             : 
+    2133        7050 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2134             : 
+    2135       14100 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2136       14100 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2137             : 
+    2138       14100 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2139             : 
+    2140       14100 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2141       14100 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2142             : 
+    2143       14100 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2144       14100 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2145             : 
+    2146             :       // if we fail in transforming the area at some point
+    2147             :       // do not publish it at all
+    2148        7050 :       bool tf_success = true;
+    2149             : 
+    2150       14100 :       geometry_msgs::TransformStamped tf = ret.value();
+    2151             : 
+    2152             :       /* transform area points to local origin //{ */
+    2153             : 
+    2154             :       // transform border bottom points to local origin
+    2155       35250 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2156             : 
+    2157       28200 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2158       28200 :         temp_ref.header.stamp         = ros::Time(0);
+    2159       28200 :         temp_ref.reference.position.x = border_points_bot_original[i].x;
+    2160       28200 :         temp_ref.reference.position.y = border_points_bot_original[i].y;
+    2161       28200 :         temp_ref.reference.position.z = border_points_bot_original[i].z;
+    2162             : 
+    2163       56400 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2164             : 
+    2165       28200 :           temp_ref = ret.value();
+    2166             : 
+    2167       28200 :           border_points_bot_transformed[i].x = temp_ref.reference.position.x;
+    2168       28200 :           border_points_bot_transformed[i].y = temp_ref.reference.position.y;
+    2169       28200 :           border_points_bot_transformed[i].z = temp_ref.reference.position.z;
+    2170             : 
+    2171             :         } else {
+    2172           0 :           tf_success = false;
+    2173             :         }
+    2174             :       }
+    2175             : 
+    2176             :       // transform border top points to local origin
+    2177       35250 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2178             : 
+    2179       28200 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2180       28200 :         temp_ref.header.stamp         = ros::Time(0);
+    2181       28200 :         temp_ref.reference.position.x = border_points_top_original[i].x;
+    2182       28200 :         temp_ref.reference.position.y = border_points_top_original[i].y;
+    2183       28200 :         temp_ref.reference.position.z = border_points_top_original[i].z;
+    2184             : 
+    2185       56400 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2186             : 
+    2187       28200 :           temp_ref = ret.value();
+    2188             : 
+    2189       28200 :           border_points_top_transformed[i].x = temp_ref.reference.position.x;
+    2190       28200 :           border_points_top_transformed[i].y = temp_ref.reference.position.y;
+    2191       28200 :           border_points_top_transformed[i].z = temp_ref.reference.position.z;
+    2192             : 
+    2193             :         } else {
+    2194           0 :           tf_success = false;
+    2195             :         }
+    2196             :       }
+    2197             : 
+    2198             :       //}
+    2199             : 
+    2200       14100 :       visualization_msgs::Marker safety_area_marker;
+    2201             : 
+    2202        7050 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2203        7050 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2204        7050 :       safety_area_marker.color.a         = 0.15;
+    2205        7050 :       safety_area_marker.scale.x         = 0.2;
+    2206        7050 :       safety_area_marker.color.r         = 1;
+    2207        7050 :       safety_area_marker.color.g         = 0;
+    2208        7050 :       safety_area_marker.color.b         = 0;
+    2209             : 
+    2210        7050 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2211             : 
+    2212       14100 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2213             : 
+    2214        7050 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2215        7050 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2216        7050 :       safety_area_coordinates_marker.color.a         = 1;
+    2217        7050 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2218        7050 :       safety_area_coordinates_marker.color.r         = 0;
+    2219        7050 :       safety_area_coordinates_marker.color.g         = 0;
+    2220        7050 :       safety_area_coordinates_marker.color.b         = 0;
+    2221             : 
+    2222        7050 :       safety_area_coordinates_marker.id = 0;
+    2223             : 
+    2224        7050 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2225             : 
+    2226             :       /* adding safety area points //{ */
+    2227             : 
+    2228             :       // bottom border
+    2229       35250 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2230             : 
+    2231       28200 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2232       28200 :         safety_area_marker.points.push_back(border_points_bot_transformed[(i + 1) % border_points_bot_transformed.size()]);
+    2233             : 
+    2234       56400 :         std::stringstream ss;
+    2235             : 
+    2236       28200 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2237           0 :           ss << "idx: " << i << std::endl
+    2238           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2239           0 :              << "lon: " << border_points_bot_original[i].y;
+    2240             :         } else {
+    2241       28200 :           ss << "idx: " << i << std::endl
+    2242       28200 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2243       28200 :              << "y: " << border_points_bot_original[i].y;
+    2244             :         }
+    2245             : 
+    2246       28200 :         safety_area_coordinates_marker.color.r = 0;
+    2247       28200 :         safety_area_coordinates_marker.color.g = 0;
+    2248       28200 :         safety_area_coordinates_marker.color.b = 0;
+    2249             : 
+    2250       28200 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed[i];
+    2251       28200 :         safety_area_coordinates_marker.text          = ss.str();
+    2252       28200 :         safety_area_coordinates_marker.id++;
+    2253             : 
+    2254       28200 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2255             :       }
+    2256             : 
+    2257             :       // top border + top/bot edges
+    2258       35250 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2259             : 
+    2260       28200 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2261       28200 :         safety_area_marker.points.push_back(border_points_top_transformed[(i + 1) % border_points_top_transformed.size()]);
+    2262             : 
+    2263       28200 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2264       28200 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2265             : 
+    2266       56400 :         std::stringstream ss;
+    2267             : 
+    2268       28200 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2269           0 :           ss << "idx: " << i << std::endl
+    2270           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2271           0 :              << "lon: " << border_points_bot_original[i].y;
+    2272             :         } else {
+    2273       28200 :           ss << "idx: " << i << std::endl
+    2274       28200 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2275       28200 :              << "y: " << border_points_bot_original[i].y;
+    2276             :         }
+    2277             : 
+    2278       28200 :         safety_area_coordinates_marker.color.r = 1;
+    2279       28200 :         safety_area_coordinates_marker.color.g = 1;
+    2280       28200 :         safety_area_coordinates_marker.color.b = 1;
+    2281             : 
+    2282       28200 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed[i];
+    2283       28200 :         safety_area_coordinates_marker.text          = ss.str();
+    2284       28200 :         safety_area_coordinates_marker.id++;
+    2285             : 
+    2286       28200 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2287             :       }
+    2288             : 
+    2289             :       //}
+    2290             : 
+    2291        7050 :       if (tf_success) {
+    2292             : 
+    2293        7050 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2294             : 
+    2295        7050 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2296             : 
+    2297        7050 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2298             :       }
+    2299             : 
+    2300             :     } else {
+    2301        1477 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2302             :     }
+    2303             :   }
+    2304             : 
+    2305             :   // --------------------------------------------------------------
+    2306             :   // |              publish the disturbances markers              |
+    2307             :   // --------------------------------------------------------------
+    2308             : 
+    2309        9545 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2310             : 
+    2311       11054 :     visualization_msgs::MarkerArray msg_out;
+    2312             : 
+    2313        5527 :     double id = 0;
+    2314             : 
+    2315        5527 :     double multiplier = 1.0;
+    2316             : 
+    2317        5527 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2318             : 
+    2319        5527 :     Eigen::Vector3d      vec3d;
+    2320        5527 :     geometry_msgs::Point point;
+    2321             : 
+    2322             :     /* world disturbance //{ */
+    2323             :     {
+    2324             : 
+    2325       11054 :       visualization_msgs::Marker marker;
+    2326             : 
+    2327        5527 :       marker.header.frame_id = uav_state.header.frame_id;
+    2328        5527 :       marker.header.stamp    = ros::Time::now();
+    2329        5527 :       marker.ns              = "control_manager";
+    2330        5527 :       marker.id              = id++;
+    2331        5527 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2332        5527 :       marker.action          = visualization_msgs::Marker::ADD;
+    2333             : 
+    2334             :       /* position //{ */
+    2335             : 
+    2336        5527 :       marker.pose.position.x = 0.0;
+    2337        5527 :       marker.pose.position.y = 0.0;
+    2338        5527 :       marker.pose.position.z = 0.0;
+    2339             : 
+    2340             :       //}
+    2341             : 
+    2342             :       /* orientation //{ */
+    2343             : 
+    2344        5527 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2345             : 
+    2346             :       //}
+    2347             : 
+    2348             :       /* origin //{ */
+    2349        5527 :       point.x = uav_x;
+    2350        5527 :       point.y = uav_y;
+    2351        5527 :       point.z = uav_z;
+    2352             : 
+    2353        5527 :       marker.points.push_back(point);
+    2354             : 
+    2355             :       //}
+    2356             : 
+    2357             :       /* tip //{ */
+    2358             : 
+    2359        5527 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2360        5527 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2361        5527 :       point.z = uav_z;
+    2362             : 
+    2363        5527 :       marker.points.push_back(point);
+    2364             : 
+    2365             :       //}
+    2366             : 
+    2367        5527 :       marker.scale.x = 0.05;
+    2368        5527 :       marker.scale.y = 0.05;
+    2369        5527 :       marker.scale.z = 0.05;
+    2370             : 
+    2371        5527 :       marker.color.a = 0.5;
+    2372        5527 :       marker.color.r = 1.0;
+    2373        5527 :       marker.color.g = 0.0;
+    2374        5527 :       marker.color.b = 0.0;
+    2375             : 
+    2376        5527 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2377             : 
+    2378        5527 :       msg_out.markers.push_back(marker);
+    2379             :     }
+    2380             : 
+    2381             :     //}
+    2382             : 
+    2383             :     /* body disturbance //{ */
+    2384             :     {
+    2385             : 
+    2386       11054 :       visualization_msgs::Marker marker;
+    2387             : 
+    2388        5527 :       marker.header.frame_id = uav_state.header.frame_id;
+    2389        5527 :       marker.header.stamp    = ros::Time::now();
+    2390        5527 :       marker.ns              = "control_manager";
+    2391        5527 :       marker.id              = id++;
+    2392        5527 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2393        5527 :       marker.action          = visualization_msgs::Marker::ADD;
+    2394             : 
+    2395             :       /* position //{ */
+    2396             : 
+    2397        5527 :       marker.pose.position.x = 0.0;
+    2398        5527 :       marker.pose.position.y = 0.0;
+    2399        5527 :       marker.pose.position.z = 0.0;
+    2400             : 
+    2401             :       //}
+    2402             : 
+    2403             :       /* orientation //{ */
+    2404             : 
+    2405        5527 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2406             : 
+    2407             :       //}
+    2408             : 
+    2409             :       /* origin //{ */
+    2410             : 
+    2411        5527 :       point.x = uav_x;
+    2412        5527 :       point.y = uav_y;
+    2413        5527 :       point.z = uav_z;
+    2414             : 
+    2415        5527 :       marker.points.push_back(point);
+    2416             : 
+    2417             :       //}
+    2418             : 
+    2419             :       /* tip //{ */
+    2420             : 
+    2421        5527 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2422        5527 :       vec3d = quat_eigen * vec3d;
+    2423             : 
+    2424        5527 :       point.x = uav_x + vec3d[0];
+    2425        5527 :       point.y = uav_y + vec3d[1];
+    2426        5527 :       point.z = uav_z + vec3d[2];
+    2427             : 
+    2428        5527 :       marker.points.push_back(point);
+    2429             : 
+    2430             :       //}
+    2431             : 
+    2432        5527 :       marker.scale.x = 0.05;
+    2433        5527 :       marker.scale.y = 0.05;
+    2434        5527 :       marker.scale.z = 0.05;
+    2435             : 
+    2436        5527 :       marker.color.a = 0.5;
+    2437        5527 :       marker.color.r = 0.0;
+    2438        5527 :       marker.color.g = 1.0;
+    2439        5527 :       marker.color.b = 0.0;
+    2440             : 
+    2441        5527 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2442             : 
+    2443        5527 :       msg_out.markers.push_back(marker);
+    2444             :     }
+    2445             : 
+    2446             :     //}
+    2447             : 
+    2448        5527 :     ph_disturbances_markers_.publish(msg_out);
+    2449             :   }
+    2450             : 
+    2451             :   // --------------------------------------------------------------
+    2452             :   // |               publish the current constraints              |
+    2453             :   // --------------------------------------------------------------
+    2454             : 
+    2455        9545 :   if (got_constraints_) {
+    2456             : 
+    2457        7900 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2458             : 
+    2459        7900 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2460             : 
+    2461        7900 :     ph_current_constraints_.publish(constraints);
+    2462             :   }
+    2463             : }
+    2464             : 
+    2465             : //}
+    2466             : 
+    2467             : /* //{ timerSafety() */
+    2468             : 
+    2469      173021 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2470             : 
+    2471      173023 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2472             : 
+    2473      173023 :   if (!is_initialized_)
+    2474           0 :     return;
+    2475             : 
+    2476      346048 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2477      346048 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2478             : 
+    2479             :   // copy member variables
+    2480      173025 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2481      173025 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2482      173025 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2483      173023 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2484      173023 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2485             : 
+    2486      327322 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2487      154299 :       active_tracker_idx == _null_tracker_idx_) {
+    2488       45099 :     return;
+    2489             :   }
+    2490             : 
+    2491      127924 :   if (odometry_switch_in_progress_) {
+    2492           4 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2493           4 :     return;
+    2494             :   }
+    2495             : 
+    2496             :   // | ------------------------ timeouts ------------------------ |
+    2497             : 
+    2498      127920 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2499      127920 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2500             : 
+    2501      127920 :     if (missing_for > _uav_state_max_missing_time_) {
+    2502           0 :       timeoutUavState(missing_for);
+    2503             :     }
+    2504             :   }
+    2505             : 
+    2506      127920 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2507           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2508             : 
+    2509           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2510           0 :       timeoutUavState(missing_for);
+    2511             :     }
+    2512             :   }
+    2513             : 
+    2514             :   // | -------------- eland and failsafe thresholds ------------- |
+    2515             : 
+    2516      127920 :   std::map<std::string, ControllerParams>::iterator it;
+    2517      127920 :   it = controllers_.find(_controller_names_[active_controller_idx]);
+    2518             : 
+    2519      127920 :   _eland_threshold_               = it->second.eland_threshold;
+    2520      127920 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2521      127920 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2522             : 
+    2523             :   // | --------- calculate control errors and tilt angle -------- |
+    2524             : 
+    2525             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2526             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2527      127920 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2528         118 :     return;
+    2529             :   }
+    2530             : 
+    2531             :   {
+    2532      127802 :     std::scoped_lock lock(mutex_attitude_error_);
+    2533             : 
+    2534      127802 :     tilt_error_ = 0;
+    2535      127802 :     yaw_error_  = 0;
+    2536             :   }
+    2537             : 
+    2538             :   {
+    2539             :     // TODO this whole scope is very clumsy
+    2540             : 
+    2541      127802 :     position_error_ = {};
+    2542             : 
+    2543      127802 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2544             : 
+    2545      126796 :       std::scoped_lock lock(mutex_position_error_);
+    2546             : 
+    2547      126794 :       if (!position_error_) {
+    2548      126785 :         position_error_ = Eigen::Vector3d::Zero(3);
+    2549             :       }
+    2550             : 
+    2551      126794 :       position_error_.value()[0] = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2552      126793 :       position_error_.value()[1] = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2553             :     }
+    2554             : 
+    2555      127800 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2556             : 
+    2557      126792 :       std::scoped_lock lock(mutex_position_error_);
+    2558             : 
+    2559      126792 :       if (!position_error_) {
+    2560           0 :         position_error_ = Eigen::Vector3d::Zero(3);
+    2561             :       }
+    2562             : 
+    2563      126792 :       position_error_.value()[2] = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2564             :     }
+    2565             :   }
+    2566             : 
+    2567             :   // rotate the drone's z axis
+    2568      127800 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2569      127800 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2570             : 
+    2571             :   // calculate the angle between the drone's z axis and the world's z axis
+    2572      127800 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2573             : 
+    2574             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2575             : 
+    2576             :   // | --------------------- the tilt error --------------------- |
+    2577             : 
+    2578      127800 :   if (last_control_output.desired_orientation) {
+    2579             : 
+    2580             :     // calculate the desired drone's z axis in the world frame
+    2581      108396 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2582      108396 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2583             : 
+    2584             :     {
+    2585      108396 :       std::scoped_lock lock(mutex_attitude_error_);
+    2586             : 
+    2587             :       // calculate the angle between the drone's z axis and the world's z axis
+    2588      108396 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2589             : 
+    2590             :       // calculate the yaw error
+    2591      108396 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2592      108396 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2593             :     }
+    2594             :   }
+    2595             : 
+    2596      127800 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2597      127800 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2598             : 
+    2599             :   // --------------------------------------------------------------
+    2600             :   // |   activate the failsafe controller in case of large error  |
+    2601             :   // --------------------------------------------------------------
+    2602             : 
+    2603      127800 :   if (position_error) {
+    2604             : 
+    2605      126792 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2606             : 
+    2607          16 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2608             : 
+    2609          16 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2610             : 
+    2611           1 :         if (!failsafe_triggered_) {
+    2612             : 
+    2613           1 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2614             :                     _failsafe_threshold_, position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2615             : 
+    2616           1 :           failsafe();
+    2617             :         }
+    2618             :       }
+    2619             :     }
+    2620             :   }
+    2621             : 
+    2622             :   // --------------------------------------------------------------
+    2623             :   // |     activate emergency land in case of large innovation    |
+    2624             :   // --------------------------------------------------------------
+    2625             : 
+    2626      127800 :   if (_odometry_innovation_check_enabled_) {
+    2627             :     {
+    2628      127800 :       auto [x, y, z] = mrs_lib::getPosition(sh_odometry_innovation_.getMsg());
+    2629             : 
+    2630      127800 :       double heading = 0;
+    2631             :       try {
+    2632      127800 :         heading = mrs_lib::getHeading(sh_odometry_innovation_.getMsg());
+    2633             :       }
+    2634           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2635           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2636             :       }
+    2637             : 
+    2638      127799 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2639             : 
+    2640      127800 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2641             : 
+    2642           2 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2643             : 
+    2644           2 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2645             : 
+    2646           1 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2647             : 
+    2648           1 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2649             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2650             : 
+    2651           1 :             eland();
+    2652             :           }
+    2653             :         }
+    2654             :       }
+    2655             :     }
+    2656             :   }
+    2657             : 
+    2658             :   // --------------------------------------------------------------
+    2659             :   // |   activate emergency land in case of medium control error  |
+    2660             :   // --------------------------------------------------------------
+    2661             : 
+    2662             :   // | ------------------- tilt control error ------------------- |
+    2663             : 
+    2664      127800 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2665             : 
+    2666           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2667             : 
+    2668           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2669             : 
+    2670           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2671             : 
+    2672           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2673             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2674             : 
+    2675           0 :         eland();
+    2676             :       }
+    2677             :     }
+    2678             :   }
+    2679             : 
+    2680             :   // | ----------------- position control error ----------------- |
+    2681             : 
+    2682      127800 :   if (position_error) {
+    2683             : 
+    2684      126792 :     double error_size = position_error->norm();
+    2685             : 
+    2686      126792 :     if (error_size > _eland_threshold_ / 2.0) {
+    2687             : 
+    2688         742 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2689             : 
+    2690         742 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2691             : 
+    2692         547 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2693             : 
+    2694         374 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2695             : 
+    2696         374 :           ungripSrv();
+    2697             :         }
+    2698             :       }
+    2699             :     }
+    2700             : 
+    2701      126792 :     if (error_size > _eland_threshold_) {
+    2702             : 
+    2703         245 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2704             : 
+    2705         245 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2706             : 
+    2707          58 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2708             : 
+    2709           1 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2710             :                     position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2711             : 
+    2712           1 :           eland();
+    2713             :         }
+    2714             :       }
+    2715             :     }
+    2716             :   }
+    2717             : 
+    2718             :   // | -------------------- yaw control error ------------------- |
+    2719             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2720             : 
+    2721      127800 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2722             : 
+    2723      127799 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2724             : 
+    2725           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2726             : 
+    2727           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2728             : 
+    2729           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2730             : 
+    2731           0 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2732             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2733             : 
+    2734           0 :           ungripSrv();
+    2735             :         }
+    2736             :       }
+    2737             :     }
+    2738             : 
+    2739      127800 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2740             : 
+    2741           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2742             : 
+    2743           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2744             : 
+    2745           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2746             : 
+    2747           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2748             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2749             : 
+    2750           0 :           eland();
+    2751             :         }
+    2752             :       }
+    2753             :     }
+    2754             :   }
+    2755             : 
+    2756             :   // --------------------------------------------------------------
+    2757             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2758             :   // --------------------------------------------------------------
+    2759      127799 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2760             : 
+    2761           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_);
+    2762             : 
+    2763           0 :     arming(false);
+    2764             :   }
+    2765             : 
+    2766             :   // --------------------------------------------------------------
+    2767             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2768             :   // --------------------------------------------------------------
+    2769             : 
+    2770      127799 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2771             : 
+    2772      127799 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2773             : 
+    2774             :     // the time from the last controller/tracker switch
+    2775             :     // fyi: we should not
+    2776      127800 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2777             : 
+    2778             :     // if the tile error is over the threshold
+    2779             :     // && we are not ramping up during takeoff
+    2780      127800 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2781             : 
+    2782             :       // only account for the error if some time passed from the last tracker/controller switch
+    2783          58 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2784             : 
+    2785             :         // if the threshold was not exceeded before
+    2786          58 :         if (!tilt_error_disarm_over_thr_) {
+    2787             : 
+    2788           1 :           tilt_error_disarm_over_thr_ = true;
+    2789           1 :           tilt_error_disarm_time_     = ros::Time::now();
+    2790             : 
+    2791           1 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2792             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2793             : 
+    2794             :           // if it was exceeded before, just keep it
+    2795             :         } else {
+    2796             : 
+    2797          57 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2798             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2799             :         }
+    2800             : 
+    2801             :         // if the tile error is bad, but the controller just switched,
+    2802             :         // don't think its bad anymore
+    2803             :       } else {
+    2804             : 
+    2805           0 :         tilt_error_disarm_over_thr_ = false;
+    2806           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2807             :       }
+    2808             : 
+    2809             :       // if the tilt error is fine
+    2810             :     } else {
+    2811             : 
+    2812             :       // make it fine
+    2813      127742 :       tilt_error_disarm_over_thr_ = false;
+    2814      127742 :       tilt_error_disarm_time_     = ros::Time::now();
+    2815             :     }
+    2816             : 
+    2817             :     // calculate the time over the threshold
+    2818      127800 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2819             : 
+    2820             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2821      127800 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2822             : 
+    2823           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2824             : 
+    2825             :       // only when flying and not in failsafe
+    2826           0 :       if (is_flying) {
+    2827             : 
+    2828           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2829             : 
+    2830           0 :         toggleOutput(false);
+    2831           0 :         arming(false);
+    2832             :       }
+    2833             :     }
+    2834             :   }
+    2835             : 
+    2836             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2837             : 
+    2838             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2839      127799 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2840             : 
+    2841           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2842             : 
+    2843           0 :     toggleOutput(false);
+    2844             :   }
+    2845             : }  // namespace control_manager
+    2846             : 
+    2847             : //}
+    2848             : 
+    2849             : /* //{ timerEland() */
+    2850             : 
+    2851         291 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2852             : 
+    2853         291 :   if (!is_initialized_)
+    2854           0 :     return;
+    2855             : 
+    2856         582 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2857         582 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2858             : 
+    2859             :   // copy member variables
+    2860         291 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2861             : 
+    2862         291 :   if (!last_control_output.control_output) {
+    2863           0 :     return;
+    2864             :   }
+    2865             : 
+    2866         291 :   auto throttle = extractThrottle(last_control_output);
+    2867             : 
+    2868         291 :   if (!throttle) {
+    2869           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
+    2870           0 :     return;
+    2871             :   }
+    2872             : 
+    2873         291 :   if (current_state_landing_ == IDLE_STATE) {
+    2874             : 
+    2875           0 :     return;
+    2876             : 
+    2877         291 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2878             : 
+    2879             :     // --------------------------------------------------------------
+    2880             :     // |                            TODO                            |
+    2881             :     // This section needs work. The throttle landing detection      |
+    2882             :     // mechanism should be extracted and other mechanisms, such     |
+    2883             :     // as velocity-based detection should be used for high          |
+    2884             :     // modalities                                                   |
+    2885             :     // --------------------------------------------------------------
+    2886             : 
+    2887         291 :     if (!last_control_output.control_output) {
+    2888           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2889           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2890           0 :       return;
+    2891             :     }
+    2892             : 
+    2893             :     // recalculate the mass based on the throttle
+    2894         291 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2895         291 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2896             : 
+    2897             :     // condition for automatic motor turn off
+    2898         291 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2899          66 :       if (!throttle_under_threshold_) {
+    2900             : 
+    2901           3 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2902           3 :         throttle_under_threshold_          = true;
+    2903             :       }
+    2904             : 
+    2905          66 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2906             : 
+    2907             :     } else {
+    2908         225 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2909         225 :       throttle_under_threshold_          = false;
+    2910             :     }
+    2911             : 
+    2912         291 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2913             :       // enable callbacks? ... NO
+    2914             : 
+    2915           3 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2916           3 :       toggleOutput(false);
+    2917             : 
+    2918             :       // disarm the drone
+    2919           3 :       if (_eland_disarm_enabled_) {
+    2920             : 
+    2921           3 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2922           3 :         arming(false);
+    2923             :       }
+    2924             : 
+    2925           3 :       changeLandingState(IDLE_STATE);
+    2926             : 
+    2927           3 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2928             : 
+    2929           3 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2930           3 :       timer_eland_.stop();
+    2931           3 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2932             : 
+    2933             :       // we should NOT set eland_triggered_=true
+    2934             :     }
+    2935             :   }
+    2936             : }
+    2937             : 
+    2938             : //}
+    2939             : 
+    2940             : /* //{ timerFailsafe() */
+    2941             : 
+    2942        9713 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2943             : 
+    2944        9713 :   if (!is_initialized_) {
+    2945           0 :     return;
+    2946             :   }
+    2947             : 
+    2948        9713 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2949             : 
+    2950       19426 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2951       19426 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2952             : 
+    2953             :   // copy member variables
+    2954        9713 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2955             : 
+    2956        9713 :   updateControllers(uav_state);
+    2957             : 
+    2958        9713 :   publish();
+    2959             : 
+    2960        9713 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2961             : 
+    2962        9713 :   if (!last_control_output.control_output) {
+    2963           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    2964           0 :     return;
+    2965             :   }
+    2966             : 
+    2967        9713 :   auto throttle = extractThrottle(last_control_output);
+    2968             : 
+    2969        9713 :   if (!throttle) {
+    2970           0 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    2971           0 :     return;
+    2972             :   }
+    2973             : 
+    2974             :   // --------------------------------------------------------------
+    2975             :   // |                            TODO                            |
+    2976             :   // This section needs work. The throttle landing detection      |
+    2977             :   // mechanism should be extracted and other mechanisms, such     |
+    2978             :   // as velocity-based detection should be used for high          |
+    2979             :   // modalities                                                   |
+    2980             :   // --------------------------------------------------------------
+    2981             : 
+    2982        9713 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2983        9713 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2984             : 
+    2985             :   // condition for automatic motor turn off
+    2986        9713 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    2987             : 
+    2988        1414 :     if (!throttle_under_threshold_) {
+    2989             : 
+    2990           7 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2991           7 :       throttle_under_threshold_          = true;
+    2992             :     }
+    2993             : 
+    2994        1414 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2995             : 
+    2996             :   } else {
+    2997             : 
+    2998        8299 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    2999        8299 :     throttle_under_threshold_          = false;
+    3000             :   }
+    3001             : 
+    3002             :   // condition for automatic motor turn off
+    3003        9713 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3004             : 
+    3005           7 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3006             : 
+    3007           7 :     arming(false);
+    3008             :   }
+    3009             : }
+    3010             : 
+    3011             : //}
+    3012             : 
+    3013             : /* //{ timerJoystick() */
+    3014             : 
+    3015       33506 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3016             : 
+    3017       33506 :   if (!is_initialized_) {
+    3018           0 :     return;
+    3019             :   }
+    3020             : 
+    3021      100518 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3022      100518 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3023             : 
+    3024       67012 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3025             : 
+    3026             :   // if start was pressed and held for > 3.0 s
+    3027       33506 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3028             : 
+    3029           0 :     joystick_start_press_time_ = ros::Time(0);
+    3030             : 
+    3031           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3032             :              _joystick_controller_name_.c_str());
+    3033             : 
+    3034           0 :     joystick_start_pressed_ = false;
+    3035             : 
+    3036           0 :     switchTracker(_joystick_tracker_name_);
+    3037           0 :     switchController(_joystick_controller_name_);
+    3038             :   }
+    3039             : 
+    3040             :   // if RT+LT were pressed and held for > 0.1 s
+    3041       33506 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3042             : 
+    3043           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3044             : 
+    3045           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3046             : 
+    3047           0 :     joystick_failsafe_pressed_ = false;
+    3048             : 
+    3049           0 :     failsafe();
+    3050             :   }
+    3051             : 
+    3052             :   // if joypads were pressed and held for > 0.1 s
+    3053       33506 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3054             : 
+    3055           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3056             : 
+    3057           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3058             : 
+    3059           0 :     joystick_failsafe_pressed_ = false;
+    3060             : 
+    3061           0 :     eland();
+    3062             :   }
+    3063             : 
+    3064             :   // if back was pressed and held for > 0.1 s
+    3065       33506 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3066             : 
+    3067           0 :     joystick_back_press_time_ = ros::Time(0);
+    3068             : 
+    3069             :     // activate/deactivate the joystick goto functionality
+    3070           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3071             : 
+    3072           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3073             :   }
+    3074             : 
+    3075             :   // if the GOTO functionality is enabled...
+    3076       33506 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3077             : 
+    3078           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3079             : 
+    3080             :     // create the reference
+    3081             : 
+    3082           0 :     mrs_msgs::Vec4::Request request;
+    3083             : 
+    3084           0 :     if (fabs(joystick_data->axes[_channel_pitch_]) >= 0.05 || fabs(joystick_data->axes[_channel_roll_]) >= 0.05 ||
+    3085           0 :         fabs(joystick_data->axes[_channel_heading_]) >= 0.05 || fabs(joystick_data->axes[_channel_throttle_]) >= 0.05) {
+    3086             : 
+    3087           0 :       if (_joystick_mode_ == 0) {
+    3088             : 
+    3089           0 :         request.goal[REF_X]       = _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * _joystick_carrot_distance_;
+    3090           0 :         request.goal[REF_Y]       = _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * _joystick_carrot_distance_;
+    3091           0 :         request.goal[REF_Z]       = _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_];
+    3092           0 :         request.goal[REF_HEADING] = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3093             : 
+    3094           0 :         mrs_msgs::Vec4::Response response;
+    3095             : 
+    3096           0 :         callbackGotoFcu(request, response);
+    3097             : 
+    3098           0 :       } else if (_joystick_mode_ == 1) {
+    3099             : 
+    3100           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3101             : 
+    3102           0 :         double dt = 0.2;
+    3103             : 
+    3104           0 :         trajectory.fly_now         = true;
+    3105           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3106           0 :         trajectory.use_heading     = true;
+    3107           0 :         trajectory.dt              = dt;
+    3108             : 
+    3109           0 :         mrs_msgs::Reference point;
+    3110           0 :         point.position.x = 0;
+    3111           0 :         point.position.y = 0;
+    3112           0 :         point.position.z = 0;
+    3113           0 :         point.heading    = 0;
+    3114             : 
+    3115           0 :         trajectory.points.push_back(point);
+    3116             : 
+    3117           0 :         double speed = 1.0;
+    3118             : 
+    3119           0 :         for (int i = 0; i < 50; i++) {
+    3120             : 
+    3121           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * (speed * dt);
+    3122           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * (speed * dt);
+    3123           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_] * (speed * dt);
+    3124           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3125             : 
+    3126           0 :           trajectory.points.push_back(point);
+    3127             :         }
+    3128             : 
+    3129           0 :         setTrajectoryReference(trajectory);
+    3130             :       }
+    3131             :     }
+    3132             :   }
+    3133             : 
+    3134       33506 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3135             : 
+    3136             :     // create the reference
+    3137        1076 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3138             : 
+    3139         538 :     double des_x       = 0;
+    3140         538 :     double des_y       = 0;
+    3141         538 :     double des_z       = 0;
+    3142         538 :     double des_heading = 0;
+    3143             : 
+    3144         538 :     bool nothing_to_do = true;
+    3145             : 
+    3146             :     // copy member variables
+    3147        1076 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3148             : 
+    3149         538 :     if (rc_channels->channels.size() < 4) {
+    3150             : 
+    3151           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3152             :                          int(rc_channels->channels.size()));
+    3153           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3154             : 
+    3155             :     } else {
+    3156             : 
+    3157         538 :       double tmp_x       = RCChannelToRange(rc_channels->channels[_rc_channel_pitch_], _rc_horizontal_speed_, 0.1);
+    3158         538 :       double tmp_y       = -RCChannelToRange(rc_channels->channels[_rc_channel_roll_], _rc_horizontal_speed_, 0.1);
+    3159         538 :       double tmp_z       = RCChannelToRange(rc_channels->channels[_rc_channel_throttle_], _rc_vertical_speed_, 0.3);
+    3160         538 :       double tmp_heading = -RCChannelToRange(rc_channels->channels[_rc_channel_heading_], _rc_heading_rate_, 0.1);
+    3161             : 
+    3162         538 :       if (abs(tmp_x) > 1e-3) {
+    3163          93 :         des_x         = tmp_x;
+    3164          93 :         nothing_to_do = false;
+    3165             :       }
+    3166             : 
+    3167         538 :       if (abs(tmp_y) > 1e-3) {
+    3168         111 :         des_y         = tmp_y;
+    3169         111 :         nothing_to_do = false;
+    3170             :       }
+    3171             : 
+    3172         538 :       if (abs(tmp_z) > 1e-3) {
+    3173         216 :         des_z         = tmp_z;
+    3174         216 :         nothing_to_do = false;
+    3175             :       }
+    3176             : 
+    3177         538 :       if (abs(tmp_heading) > 1e-3) {
+    3178          48 :         des_heading   = tmp_heading;
+    3179          48 :         nothing_to_do = false;
+    3180             :       }
+    3181             :     }
+    3182             : 
+    3183         538 :     if (!nothing_to_do) {
+    3184             : 
+    3185         468 :       request.reference.header.frame_id = "fcu_untilted";
+    3186             : 
+    3187         468 :       request.reference.reference.use_heading_rate = true;
+    3188             : 
+    3189         468 :       request.reference.reference.velocity.x   = des_x;
+    3190         468 :       request.reference.reference.velocity.y   = des_y;
+    3191         468 :       request.reference.reference.velocity.z   = des_z;
+    3192         468 :       request.reference.reference.heading_rate = des_heading;
+    3193             : 
+    3194         936 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3195             : 
+    3196             :       // disable callbacks of all trackers
+    3197         468 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3198             : 
+    3199             :       // enable the callbacks for the active tracker
+    3200         468 :       req_enable_callbacks.data = true;
+    3201             :       {
+    3202         468 :         std::scoped_lock lock(mutex_tracker_list_);
+    3203             : 
+    3204         936 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3205         936 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3206             :       }
+    3207             : 
+    3208         468 :       callbacks_enabled_ = true;
+    3209             : 
+    3210         468 :       callbackVelocityReferenceService(request, response);
+    3211             : 
+    3212         468 :       callbacks_enabled_ = false;
+    3213             : 
+    3214         468 :       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);
+    3215             : 
+    3216             :       // disable the callbacks back again
+    3217         468 :       req_enable_callbacks.data = false;
+    3218             :       {
+    3219         468 :         std::scoped_lock lock(mutex_tracker_list_);
+    3220             : 
+    3221         936 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3222         936 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3223             :       }
+    3224             :     }
+    3225             :   }
+    3226             : }
+    3227             : 
+    3228             : //}
+    3229             : 
+    3230             : /* //{ timerBumper() */
+    3231             : 
+    3232           0 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3233             : 
+    3234           0 :   if (!is_initialized_)
+    3235           0 :     return;
+    3236             : 
+    3237           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3238           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3239             : 
+    3240           0 :   if (!bumper_enabled_) {
+    3241           0 :     return;
+    3242             :   }
+    3243             : 
+    3244           0 :   if (!isFlyingNormally()) {
+    3245           0 :     return;
+    3246             :   }
+    3247             : 
+    3248           0 :   if (!got_uav_state_) {
+    3249           0 :     return;
+    3250             :   }
+    3251             : 
+    3252           0 :   if ((ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3253           0 :     return;
+    3254             :   }
+    3255             : 
+    3256             :   // --------------------------------------------------------------
+    3257             :   // |                      bumper repulsion                      |
+    3258             :   // --------------------------------------------------------------
+    3259             : 
+    3260           0 :   bumperPushFromObstacle();
+    3261             : }
+    3262             : 
+    3263             : //}
+    3264             : 
+    3265             : /* //{ timerPirouette() */
+    3266             : 
+    3267           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3268             : 
+    3269           0 :   if (!is_initialized_)
+    3270           0 :     return;
+    3271             : 
+    3272           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3273           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3274             : 
+    3275           0 :   pirouette_iterator_++;
+    3276             : 
+    3277           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3278           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3279           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3280             : 
+    3281           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3282             : 
+    3283           0 :     _pirouette_enabled_ = false;
+    3284           0 :     timer_pirouette_.stop();
+    3285             : 
+    3286           0 :     setCallbacks(true);
+    3287             : 
+    3288           0 :     return;
+    3289             :   }
+    3290             : 
+    3291             :   // set the reference
+    3292           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3293             : 
+    3294           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3295             : 
+    3296           0 :   reference_request.header.frame_id      = "";
+    3297           0 :   reference_request.header.stamp         = ros::Time(0);
+    3298           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3299           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3300           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3301           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3302             : 
+    3303             :   // enable the callbacks for the active tracker
+    3304             :   {
+    3305           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3306             : 
+    3307           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3308           0 :     req_enable_callbacks.data = true;
+    3309             : 
+    3310           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3311             : 
+    3312           0 :     callbacks_enabled_ = true;
+    3313             :   }
+    3314             : 
+    3315           0 :   setReference(reference_request);
+    3316             : 
+    3317             :   {
+    3318           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3319             : 
+    3320             :     // disable the callbacks for the active tracker
+    3321           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3322           0 :     req_enable_callbacks.data = false;
+    3323             : 
+    3324           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3325             : 
+    3326           0 :     callbacks_enabled_ = false;
+    3327             :   }
+    3328             : }
+    3329             : 
+    3330             : //}
+    3331             : 
+    3332             : // --------------------------------------------------------------
+    3333             : // |                           asyncs                           |
+    3334             : // --------------------------------------------------------------
+    3335             : 
+    3336             : /* asyncControl() //{ */
+    3337             : 
+    3338       73885 : void ControlManager::asyncControl(void) {
+    3339             : 
+    3340       73885 :   if (!is_initialized_)
+    3341           0 :     return;
+    3342             : 
+    3343      147770 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3344             : 
+    3345      221655 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3346      221655 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3347             : 
+    3348             :   // copy member variables
+    3349      147770 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3350       73885 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3351             : 
+    3352       73885 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3353             : 
+    3354             :     // run the safety timer
+    3355             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3356       64643 :     while (running_safety_timer_) {
+    3357             : 
+    3358         611 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3359         611 :       ros::Duration wait(0.001);
+    3360         611 :       wait.sleep();
+    3361             : 
+    3362         611 :       if (!running_safety_timer_) {
+    3363         608 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3364         608 :         break;
+    3365             :       }
+    3366             :     }
+    3367             : 
+    3368       64640 :     ros::TimerEvent safety_timer_event;
+    3369       64640 :     timerSafety(safety_timer_event);
+    3370             : 
+    3371       64638 :     updateTrackers();
+    3372             : 
+    3373       64638 :     updateControllers(uav_state);
+    3374             : 
+    3375       64638 :     if (got_constraints_) {
+    3376             : 
+    3377             :       // update the constraints to trackers, if need to
+    3378       64209 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3379             : 
+    3380       64209 :       if (enforce && !constraints_being_enforced_) {
+    3381             : 
+    3382          36 :         setConstraintsToTrackers(enforce.value());
+    3383          36 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3384             : 
+    3385          36 :         constraints_being_enforced_ = true;
+    3386             : 
+    3387          36 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3388             : 
+    3389          36 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3390             :                           _controller_names_[active_controller_idx].c_str());
+    3391             : 
+    3392       64173 :       } else if (!enforce && constraints_being_enforced_) {
+    3393             : 
+    3394          32 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3395             : 
+    3396          32 :         constraints_being_enforced_ = false;
+    3397             : 
+    3398          32 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3399             : 
+    3400          32 :         setConstraintsToTrackers(current_constraints);
+    3401             :       }
+    3402             :     }
+    3403             : 
+    3404       64638 :     publish();
+    3405             :   }
+    3406             : 
+    3407             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3408       73883 :   if (odometry_switch_in_progress_) {
+    3409             : 
+    3410           4 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3411           4 :     timer_safety_.start();
+    3412           4 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3413           4 :     odometry_switch_in_progress_ = false;
+    3414             : 
+    3415             :     {
+    3416           8 :       std::scoped_lock lock(mutex_uav_state_);
+    3417             : 
+    3418           4 :       ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    3419             :                uav_state.pose.position.z, uav_heading_);
+    3420             :     }
+    3421             :   }
+    3422             : }
+    3423             : 
+    3424             : //}
+    3425             : 
+    3426             : // --------------------------------------------------------------
+    3427             : // |                          callbacks                         |
+    3428             : // --------------------------------------------------------------
+    3429             : 
+    3430             : // | --------------------- topic callbacks -------------------- |
+    3431             : 
+    3432             : /* //{ callbackOdometry() */
+    3433             : 
+    3434           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3435             : 
+    3436           0 :   if (!is_initialized_)
+    3437           0 :     return;
+    3438             : 
+    3439           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3440           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3441             : 
+    3442           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3443             : 
+    3444             :   // | ------------------ check for time stamp ------------------ |
+    3445             : 
+    3446             :   {
+    3447           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3448             : 
+    3449           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3450           0 :       return;
+    3451             :     }
+    3452             :   }
+    3453             : 
+    3454             :   // | --------------------- check for nans --------------------- |
+    3455             : 
+    3456           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3457           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3458           0 :     return;
+    3459             :   }
+    3460             : 
+    3461             :   // | ---------------------- frame switch ---------------------- |
+    3462             : 
+    3463             :   /* Odometry frame switch //{ */
+    3464             : 
+    3465             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3466             : 
+    3467           0 :   mrs_msgs::UavState uav_state_odom;
+    3468             : 
+    3469           0 :   uav_state_odom.header   = odom->header;
+    3470           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3471           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3472             : 
+    3473             :   // | ----- check for change in odometry frame of reference ---- |
+    3474             : 
+    3475           0 :   if (got_uav_state_) {
+    3476             : 
+    3477           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3478             : 
+    3479           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3480             :       {
+    3481           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3482             : 
+    3483           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,
+    3484             :                  uav_state_.pose.position.z, uav_heading_);
+    3485             :       }
+    3486             : 
+    3487           0 :       odometry_switch_in_progress_ = true;
+    3488             : 
+    3489             :       // we have to stop safety timer, otherwise it will interfere
+    3490           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3491           0 :       timer_safety_.stop();
+    3492           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3493             : 
+    3494             :       // wait for the safety timer to stop if its running
+    3495           0 :       while (running_safety_timer_) {
+    3496             : 
+    3497           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3498           0 :         ros::Duration wait(0.001);
+    3499           0 :         wait.sleep();
+    3500             : 
+    3501           0 :         if (!running_safety_timer_) {
+    3502           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3503           0 :           break;
+    3504             :         }
+    3505             :       }
+    3506             : 
+    3507             :       // we have to also for the oneshot control timer to finish
+    3508           0 :       while (running_async_control_) {
+    3509             : 
+    3510           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3511           0 :         ros::Duration wait(0.001);
+    3512           0 :         wait.sleep();
+    3513             : 
+    3514           0 :         if (!running_async_control_) {
+    3515           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3516           0 :           break;
+    3517             :         }
+    3518             :       }
+    3519             : 
+    3520             :       {
+    3521           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3522             : 
+    3523           0 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(uav_state_odom);
+    3524           0 :         controller_list_[active_controller_idx_]->switchOdometrySource(uav_state_odom);
+    3525             :       }
+    3526             :     }
+    3527             :   }
+    3528             : 
+    3529             :   //}
+    3530             : 
+    3531             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3532             : 
+    3533             :   {
+    3534           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3535             : 
+    3536           0 :     previous_uav_state_ = uav_state_;
+    3537             : 
+    3538           0 :     uav_state_ = mrs_msgs::UavState();
+    3539             : 
+    3540           0 :     uav_state_.header           = odom->header;
+    3541           0 :     uav_state_.pose             = odom->pose.pose;
+    3542           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3543             : 
+    3544             :     // transform the twist into the header's frame
+    3545             :     {
+    3546             :       // the velocity from the odometry
+    3547           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3548           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3549           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3550           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3551           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3552           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3553             : 
+    3554           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3555             : 
+    3556           0 :       if (res) {
+    3557           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3558           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3559           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3560             :       } else {
+    3561           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3562             :                            odom->header.frame_id.c_str());
+    3563           0 :         return;
+    3564             :       }
+    3565             :     }
+    3566             : 
+    3567             :     // calculate the euler angles
+    3568           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3569             : 
+    3570             :     try {
+    3571           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3572             :     }
+    3573           0 :     catch (...) {
+    3574           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3575             :     }
+    3576             : 
+    3577           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3578             : 
+    3579           0 :     got_uav_state_ = true;
+    3580             :   }
+    3581             : 
+    3582             :   // run the control loop asynchronously in an OneShotTimer
+    3583             :   // but only if its not already running
+    3584           0 :   if (!running_async_control_) {
+    3585             : 
+    3586           0 :     running_async_control_ = true;
+    3587             : 
+    3588           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3589             :   }
+    3590             : }
+    3591             : 
+    3592             : //}
+    3593             : 
+    3594             : /* //{ callbackUavState() */
+    3595             : 
+    3596       94453 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3597             : 
+    3598       94453 :   if (!is_initialized_)
+    3599       20088 :     return;
+    3600             : 
+    3601      188906 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3602      188906 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3603             : 
+    3604       94453 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3605             : 
+    3606             :   // | ------------------ check for time stamp ------------------ |
+    3607             : 
+    3608             :   {
+    3609       94453 :     std::scoped_lock lock(mutex_uav_state_);
+    3610             : 
+    3611       94453 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3612       20088 :       return;
+    3613             :     }
+    3614             :   }
+    3615             : 
+    3616             :   // | --------------------- check for nans --------------------- |
+    3617             : 
+    3618       74365 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3619           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3620           0 :     return;
+    3621             :   }
+    3622             : 
+    3623             :   // | -------------------- check for hiccups ------------------- |
+    3624             : 
+    3625             :   /* hickup detection //{ */
+    3626             : 
+    3627       74365 :   double alpha               = 0.99;
+    3628       74365 :   double alpha2              = 0.666;
+    3629       74365 :   double uav_state_count_lim = 1000;
+    3630             : 
+    3631       74365 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3632             : 
+    3633             :   // belive only reasonable numbers
+    3634       74365 :   if (uav_state_dt <= 1.0) {
+    3635             : 
+    3636       74251 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3637             : 
+    3638       74251 :     if (uav_state_count_ < uav_state_count_lim) {
+    3639       47640 :       uav_state_count_++;
+    3640             :     }
+    3641             :   }
+    3642             : 
+    3643       74365 :   if (uav_state_count_ == uav_state_count_lim) {
+    3644             : 
+    3645             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3646             : 
+    3647       26649 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3648             : 
+    3649       11094 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3650             : 
+    3651       15555 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3652             : 
+    3653       15555 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3654             :     }
+    3655             : 
+    3656       26649 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3657             : 
+    3658             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3659             : 
+    3660           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3661           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3662           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3663           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3664           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3665           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3666           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3667           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3668           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3669           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3670             :     }
+    3671             :   }
+    3672             : 
+    3673             :   //}
+    3674             : 
+    3675             :   // | ---------------------- frame switch ---------------------- |
+    3676             : 
+    3677             :   /* frame switch //{ */
+    3678             : 
+    3679             :   // | ----- check for change in odometry frame of reference ---- |
+    3680             : 
+    3681       74365 :   if (got_uav_state_) {
+    3682             : 
+    3683       74308 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3684             : 
+    3685           4 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3686             :       {
+    3687           8 :         std::scoped_lock lock(mutex_uav_state_);
+    3688             : 
+    3689           4 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3690             :                  uav_state_.pose.position.z, uav_heading_);
+    3691             :       }
+    3692             : 
+    3693           4 :       odometry_switch_in_progress_ = true;
+    3694             : 
+    3695             :       // we have to stop safety timer, otherwise it will interfere
+    3696           4 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3697           4 :       timer_safety_.stop();
+    3698           4 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3699             : 
+    3700             :       // wait for the safety timer to stop if its running
+    3701           4 :       while (running_safety_timer_) {
+    3702             : 
+    3703           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3704           0 :         ros::Duration wait(0.001);
+    3705           0 :         wait.sleep();
+    3706             : 
+    3707           0 :         if (!running_safety_timer_) {
+    3708           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3709           0 :           break;
+    3710             :         }
+    3711             :       }
+    3712             : 
+    3713             :       // we have to also for the oneshot control timer to finish
+    3714           4 :       while (running_async_control_) {
+    3715             : 
+    3716           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3717           0 :         ros::Duration wait(0.001);
+    3718           0 :         wait.sleep();
+    3719             : 
+    3720           0 :         if (!running_async_control_) {
+    3721           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3722           0 :           break;
+    3723             :         }
+    3724             :       }
+    3725             : 
+    3726             :       {
+    3727           8 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3728             : 
+    3729           4 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(*uav_state);
+    3730           4 :         controller_list_[active_controller_idx_]->switchOdometrySource(*uav_state);
+    3731             :       }
+    3732             :     }
+    3733             :   }
+    3734             : 
+    3735             :   //}
+    3736             : 
+    3737             :   // --------------------------------------------------------------
+    3738             :   // |           copy the UavState message for later use          |
+    3739             :   // --------------------------------------------------------------
+    3740             : 
+    3741             :   {
+    3742       74365 :     std::scoped_lock lock(mutex_uav_state_);
+    3743             : 
+    3744       74365 :     previous_uav_state_ = uav_state_;
+    3745             : 
+    3746       74365 :     uav_state_ = *uav_state;
+    3747             : 
+    3748       74365 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3749             : 
+    3750             :     try {
+    3751       74365 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3752             :     }
+    3753           0 :     catch (...) {
+    3754           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3755             :     }
+    3756             : 
+    3757       74365 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3758             : 
+    3759       74365 :     got_uav_state_ = true;
+    3760             :   }
+    3761             : 
+    3762             :   // run the control loop asynchronously in an OneShotTimer
+    3763             :   // but only if its not already running
+    3764       74365 :   if (!running_async_control_) {
+    3765             : 
+    3766       73885 :     running_async_control_ = true;
+    3767             : 
+    3768       73885 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3769             :   }
+    3770             : }
+    3771             : 
+    3772             : //}
+    3773             : 
+    3774             : /* //{ callbackGNSS() */
+    3775             : 
+    3776       53523 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3777             : 
+    3778       53523 :   if (!is_initialized_)
+    3779          68 :     return;
+    3780             : 
+    3781      160365 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3782      160365 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3783             : 
+    3784       53455 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3785             : }
+    3786             : 
+    3787             : //}
+    3788             : 
+    3789             : /* callbackJoystick() //{ */
+    3790             : 
+    3791           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3792             : 
+    3793           0 :   if (!is_initialized_)
+    3794           0 :     return;
+    3795             : 
+    3796           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3797           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3798             : 
+    3799             :   // copy member variables
+    3800           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3801           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3802             : 
+    3803           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3804             : 
+    3805             :   // TODO check if the array is smaller than the largest idx
+    3806           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3807           0 :     return;
+    3808             :   }
+    3809             : 
+    3810             :   // | ---- switching back to fallback tracker and controller --- |
+    3811             : 
+    3812             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3813           0 :   if ((joystick_data->buttons[_channel_A_] == 1 || joystick_data->buttons[_channel_B_] == 1 || joystick_data->buttons[_channel_X_] == 1 ||
+    3814           0 :        joystick_data->buttons[_channel_Y_] == 1) &&
+    3815           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3816             : 
+    3817           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3818             : 
+    3819           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3820           0 :     switchController(_joystick_fallback_controller_name_);
+    3821             : 
+    3822           0 :     joystick_goto_enabled_ = false;
+    3823             :   }
+    3824             : 
+    3825             :   // | ------- joystick control activation ------- |
+    3826             : 
+    3827             :   // if start button was pressed
+    3828           0 :   if (joystick_data->buttons[_channel_start_] == 1) {
+    3829             : 
+    3830           0 :     if (!joystick_start_pressed_) {
+    3831             : 
+    3832           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3833             : 
+    3834           0 :       joystick_start_pressed_    = true;
+    3835           0 :       joystick_start_press_time_ = ros::Time::now();
+    3836             :     }
+    3837             : 
+    3838           0 :   } else if (joystick_start_pressed_) {
+    3839             : 
+    3840           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3841             : 
+    3842           0 :     joystick_start_pressed_    = false;
+    3843           0 :     joystick_start_press_time_ = ros::Time(0);
+    3844             :   }
+    3845             : 
+    3846             :   // | ---------------- Joystick goto activation ---------------- |
+    3847             : 
+    3848             :   // if back button was pressed
+    3849           0 :   if (joystick_data->buttons[_channel_back_] == 1) {
+    3850             : 
+    3851           0 :     if (!joystick_back_pressed_) {
+    3852             : 
+    3853           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3854             : 
+    3855           0 :       joystick_back_pressed_    = true;
+    3856           0 :       joystick_back_press_time_ = ros::Time::now();
+    3857             :     }
+    3858             : 
+    3859           0 :   } else if (joystick_back_pressed_) {
+    3860             : 
+    3861           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3862             : 
+    3863           0 :     joystick_back_pressed_    = false;
+    3864           0 :     joystick_back_press_time_ = ros::Time(0);
+    3865             :   }
+    3866             : 
+    3867             :   // | ------------------------ Failsafes ----------------------- |
+    3868             : 
+    3869             :   // if LT and RT buttons are both pressed down
+    3870           0 :   if (joystick_data->axes[_channel_LT_] < -0.99 && joystick_data->axes[_channel_RT_] < -0.99) {
+    3871             : 
+    3872           0 :     if (!joystick_failsafe_pressed_) {
+    3873             : 
+    3874           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3875             : 
+    3876           0 :       joystick_failsafe_pressed_    = true;
+    3877           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3878             :     }
+    3879             : 
+    3880           0 :   } else if (joystick_failsafe_pressed_) {
+    3881             : 
+    3882           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3883             : 
+    3884           0 :     joystick_failsafe_pressed_    = false;
+    3885           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3886             :   }
+    3887             : 
+    3888             :   // if left and right joypads are both pressed down
+    3889           0 :   if (joystick_data->buttons[_channel_L_joy_] == 1 && joystick_data->buttons[_channel_R_joy_] == 1) {
+    3890             : 
+    3891           0 :     if (!joystick_eland_pressed_) {
+    3892             : 
+    3893           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3894             : 
+    3895           0 :       joystick_eland_pressed_    = true;
+    3896           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3897             :     }
+    3898             : 
+    3899           0 :   } else if (joystick_eland_pressed_) {
+    3900             : 
+    3901           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3902             : 
+    3903           0 :     joystick_eland_pressed_    = false;
+    3904           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3905             :   }
+    3906             : }
+    3907             : 
+    3908             : //}
+    3909             : 
+    3910             : /* //{ callbackHwApiStatus() */
+    3911             : 
+    3912      222997 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3913             : 
+    3914      222997 :   if (!is_initialized_)
+    3915         192 :     return;
+    3916             : 
+    3917      668415 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3918      668415 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3919             : 
+    3920      445610 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3921             : 
+    3922             :   // | ------ detect and print the changes in offboard mode ----- |
+    3923      222805 :   if (state->offboard) {
+    3924             : 
+    3925      165459 :     if (!offboard_mode_) {
+    3926          54 :       offboard_mode_          = true;
+    3927          54 :       offboard_mode_was_true_ = true;
+    3928          54 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3929             :     }
+    3930             : 
+    3931             :   } else {
+    3932             : 
+    3933       57346 :     if (offboard_mode_) {
+    3934           0 :       offboard_mode_ = false;
+    3935           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3936             :     }
+    3937             :   }
+    3938             : 
+    3939             :   // | --------- detect and print the changes in arming --------- |
+    3940      222805 :   if (state->armed == true) {
+    3941             : 
+    3942      170440 :     if (!armed_) {
+    3943          57 :       armed_ = true;
+    3944          57 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    3945             :     }
+    3946             : 
+    3947             :   } else {
+    3948             : 
+    3949       52365 :     if (armed_) {
+    3950          16 :       armed_ = false;
+    3951          16 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    3952             :     }
+    3953             :   }
+    3954             : }
+    3955             : 
+    3956             : //}
+    3957             : 
+    3958             : /* //{ callbackRC() */
+    3959             : 
+    3960       15495 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    3961             : 
+    3962       15495 :   if (!is_initialized_)
+    3963           0 :     return;
+    3964             : 
+    3965       46485 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    3966       46485 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    3967             : 
+    3968       30990 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    3969             : 
+    3970       15495 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    3971             : 
+    3972             :   // | ------------------- rc joystic control ------------------- |
+    3973             : 
+    3974             :   // when the switch change its position
+    3975       15495 :   if (_rc_goto_enabled_) {
+    3976             : 
+    3977       15495 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    3978             : 
+    3979           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    3980             :                          int(rc->channels.size()));
+    3981             : 
+    3982             :     } else {
+    3983             : 
+    3984       15495 :       bool channel_low  = rc->channels[_rc_joystick_channel_] < (0.5 - RC_DEADBAND) ? true : false;
+    3985       15495 :       bool channel_high = rc->channels[_rc_joystick_channel_] > (0.5 + RC_DEADBAND) ? true : false;
+    3986             : 
+    3987       15495 :       if (channel_low) {
+    3988       13724 :         rc_joystick_channel_was_low_ = true;
+    3989             :       }
+    3990             : 
+    3991             :       // rc control activation
+    3992       15495 :       if (!rc_goto_active_) {
+    3993             : 
+    3994       13725 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    3995             : 
+    3996           2 :           if (isFlyingNormally()) {
+    3997             : 
+    3998           2 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    3999             : 
+    4000           2 :             callbacks_enabled_ = false;
+    4001             : 
+    4002           2 :             std_srvs::SetBoolRequest req_goto_out;
+    4003           2 :             req_goto_out.data = false;
+    4004             : 
+    4005           2 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4006           2 :             req_enable_callbacks.data = callbacks_enabled_;
+    4007             : 
+    4008             :             {
+    4009           4 :               std::scoped_lock lock(mutex_tracker_list_);
+    4010             : 
+    4011             :               // disable callbacks of all trackers
+    4012          14 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4013          12 :                 tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4014             :               }
+    4015             :             }
+    4016             : 
+    4017           2 :             rc_goto_active_ = true;
+    4018             : 
+    4019             :           } else {
+    4020             : 
+    4021           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4022           2 :           }
+    4023             : 
+    4024       13723 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4025             : 
+    4026           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4027             :         }
+    4028             :       }
+    4029             : 
+    4030             :       // rc control deactivation
+    4031       15495 :       if (rc_goto_active_ && channel_low) {
+    4032             : 
+    4033           1 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4034             : 
+    4035           1 :         callbacks_enabled_ = true;
+    4036             : 
+    4037           1 :         std_srvs::SetBoolRequest req_goto_out;
+    4038           1 :         req_goto_out.data = true;
+    4039             : 
+    4040           1 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4041           1 :         req_enable_callbacks.data = callbacks_enabled_;
+    4042             : 
+    4043             :         {
+    4044           2 :           std::scoped_lock lock(mutex_tracker_list_);
+    4045             : 
+    4046             :           // enable callbacks of all trackers
+    4047           7 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4048           6 :             tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4049             :           }
+    4050             :         }
+    4051             : 
+    4052           1 :         rc_goto_active_ = false;
+    4053             :       }
+    4054             : 
+    4055             :       // do not forget to update the last... variable
+    4056             :       // only do that if its out of the deadband
+    4057       15495 :       if (channel_high || channel_low) {
+    4058       15495 :         rc_joystick_channel_last_value_ = rc->channels[_rc_joystick_channel_];
+    4059             :       }
+    4060             :     }
+    4061             :   }
+    4062             : 
+    4063             :   // | ------------------------ rc eland ------------------------ |
+    4064       15495 :   if (_rc_escalating_failsafe_enabled_) {
+    4065             : 
+    4066       15495 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4067             : 
+    4068           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4069             :                          int(rc->channels.size()));
+    4070             : 
+    4071             :     } else {
+    4072             : 
+    4073       15495 :       if (rc->channels[_rc_escalating_failsafe_channel_] >= _rc_escalating_failsafe_threshold_) {
+    4074             : 
+    4075         139 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4076             : 
+    4077         278 :         auto [success, message] = escalatingFailsafe();
+    4078             : 
+    4079         139 :         if (success) {
+    4080           3 :           rc_escalating_failsafe_triggered_ = true;
+    4081             :         }
+    4082             :       }
+    4083             :     }
+    4084             :   }
+    4085             : }
+    4086             : 
+    4087             : //}
+    4088             : 
+    4089             : // | --------------------- topic timeouts --------------------- |
+    4090             : 
+    4091             : /* timeoutUavState() //{ */
+    4092             : 
+    4093           0 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4094             : 
+    4095           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4096             : 
+    4097           0 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4098             : 
+    4099             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4100             :     // in place of the callbackUavState/callbackOdometry().
+    4101             : 
+    4102           0 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4103             : 
+    4104           0 :     failsafe();
+    4105             :   }
+    4106           0 : }
+    4107             : 
+    4108             : //}
+    4109             : 
+    4110             : // | -------------------- service callbacks ------------------- |
+    4111             : 
+    4112             : /* //{ callbackSwitchTracker() */
+    4113             : 
+    4114         114 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4115             : 
+    4116         114 :   if (!is_initialized_)
+    4117           0 :     return false;
+    4118             : 
+    4119         114 :   if (failsafe_triggered_ || eland_triggered_) {
+    4120             : 
+    4121           0 :     std::stringstream ss;
+    4122           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4123             : 
+    4124           0 :     res.message = ss.str();
+    4125           0 :     res.success = false;
+    4126             : 
+    4127           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4128             : 
+    4129           0 :     return true;
+    4130             :   }
+    4131             : 
+    4132         114 :   auto [success, response] = switchTracker(req.value);
+    4133             : 
+    4134         114 :   res.success = success;
+    4135         114 :   res.message = response;
+    4136             : 
+    4137         114 :   return true;
+    4138             : }
+    4139             : 
+    4140             : //}
+    4141             : 
+    4142             : /* callbackSwitchController() //{ */
+    4143             : 
+    4144         111 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4145             : 
+    4146         111 :   if (!is_initialized_)
+    4147           0 :     return false;
+    4148             : 
+    4149         111 :   if (failsafe_triggered_ || eland_triggered_) {
+    4150             : 
+    4151           0 :     std::stringstream ss;
+    4152           0 :     ss << "can not switch controller, eland or failsafe active";
+    4153             : 
+    4154           0 :     res.message = ss.str();
+    4155           0 :     res.success = false;
+    4156             : 
+    4157           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4158             : 
+    4159           0 :     return true;
+    4160             :   }
+    4161             : 
+    4162         111 :   auto [success, response] = switchController(req.value);
+    4163             : 
+    4164         111 :   res.success = success;
+    4165         111 :   res.message = response;
+    4166             : 
+    4167         111 :   return true;
+    4168             : }
+    4169             : 
+    4170             : //}
+    4171             : 
+    4172             : /* //{ callbackSwitchTracker() */
+    4173             : 
+    4174           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4175             : 
+    4176           0 :   if (!is_initialized_)
+    4177           0 :     return false;
+    4178             : 
+    4179           0 :   std::stringstream message;
+    4180             : 
+    4181           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4182             : 
+    4183           0 :     message << "can not reset tracker, eland or failsafe active";
+    4184             : 
+    4185           0 :     res.message = message.str();
+    4186           0 :     res.success = false;
+    4187             : 
+    4188           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4189             : 
+    4190           0 :     return true;
+    4191             :   }
+    4192             : 
+    4193             :   // reactivate the current tracker
+    4194             :   {
+    4195           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4196             : 
+    4197           0 :     std::string tracker_name = _tracker_names_[active_tracker_idx_];
+    4198             : 
+    4199           0 :     bool succ = tracker_list_[active_tracker_idx_]->resetStatic();
+    4200             : 
+    4201           0 :     if (succ) {
+    4202           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4203           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4204             :     } else {
+    4205           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4206           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4207             :     }
+    4208             :   }
+    4209             : 
+    4210           0 :   res.message = message.str();
+    4211           0 :   res.success = true;
+    4212             : 
+    4213           0 :   return true;
+    4214             : }
+    4215             : 
+    4216             : //}
+    4217             : 
+    4218             : /* //{ callbackEHover() */
+    4219             : 
+    4220           0 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4221             : 
+    4222           0 :   if (!is_initialized_)
+    4223           0 :     return false;
+    4224             : 
+    4225           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4226             : 
+    4227           0 :     std::stringstream ss;
+    4228           0 :     ss << "can not switch controller, eland or failsafe active";
+    4229             : 
+    4230           0 :     res.message = ss.str();
+    4231           0 :     res.success = false;
+    4232             : 
+    4233           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4234             : 
+    4235           0 :     return true;
+    4236             :   }
+    4237             : 
+    4238           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4239             : 
+    4240           0 :   auto [success, message] = ehover();
+    4241             : 
+    4242           0 :   res.success = success;
+    4243           0 :   res.message = message;
+    4244             : 
+    4245           0 :   return true;
+    4246             : }
+    4247             : 
+    4248             : //}
+    4249             : 
+    4250             : /* callbackFailsafe() //{ */
+    4251             : 
+    4252           4 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4253             : 
+    4254           4 :   if (!is_initialized_)
+    4255           0 :     return false;
+    4256             : 
+    4257           4 :   if (failsafe_triggered_) {
+    4258             : 
+    4259           0 :     std::stringstream ss;
+    4260           0 :     ss << "can not activate failsafe, it is already active";
+    4261             : 
+    4262           0 :     res.message = ss.str();
+    4263           0 :     res.success = false;
+    4264             : 
+    4265           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4266             : 
+    4267           0 :     return true;
+    4268             :   }
+    4269             : 
+    4270           4 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4271             : 
+    4272           4 :   auto [success, message] = failsafe();
+    4273             : 
+    4274           4 :   res.success = success;
+    4275           4 :   res.message = message;
+    4276             : 
+    4277           4 :   return true;
+    4278             : }
+    4279             : 
+    4280             : //}
+    4281             : 
+    4282             : /* callbackFailsafeEscalating() //{ */
+    4283             : 
+    4284           7 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4285             : 
+    4286           7 :   if (!is_initialized_)
+    4287           0 :     return false;
+    4288             : 
+    4289           7 :   if (_service_escalating_failsafe_enabled_) {
+    4290             : 
+    4291           7 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4292             : 
+    4293          14 :     auto [success, message] = escalatingFailsafe();
+    4294             : 
+    4295           7 :     res.success = success;
+    4296           7 :     res.message = message;
+    4297             : 
+    4298             :   } else {
+    4299             : 
+    4300           0 :     std::stringstream ss;
+    4301           0 :     ss << "escalating failsafe is disabled";
+    4302             : 
+    4303           0 :     res.success = false;
+    4304           0 :     res.message = ss.str();
+    4305             : 
+    4306           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4307             :   }
+    4308             : 
+    4309           7 :   return true;
+    4310             : }
+    4311             : 
+    4312             : //}
+    4313             : 
+    4314             : /* //{ callbackELand() */
+    4315             : 
+    4316           1 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4317             : 
+    4318           1 :   if (!is_initialized_)
+    4319           0 :     return false;
+    4320             : 
+    4321           1 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4322             : 
+    4323           1 :   auto [success, message] = eland();
+    4324             : 
+    4325           1 :   res.success = success;
+    4326           1 :   res.message = message;
+    4327             : 
+    4328           1 :   return true;
+    4329             : }
+    4330             : 
+    4331             : //}
+    4332             : 
+    4333             : /* //{ callbackParachute() */
+    4334             : 
+    4335           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4336             : 
+    4337           0 :   if (!is_initialized_)
+    4338           0 :     return false;
+    4339             : 
+    4340           0 :   if (!_parachute_enabled_) {
+    4341             : 
+    4342           0 :     std::stringstream ss;
+    4343           0 :     ss << "parachute disabled";
+    4344           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4345           0 :     res.message = ss.str();
+    4346           0 :     res.success = false;
+    4347             :   }
+    4348             : 
+    4349           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4350             : 
+    4351           0 :   auto [success, message] = deployParachute();
+    4352             : 
+    4353           0 :   res.success = success;
+    4354           0 :   res.message = message;
+    4355             : 
+    4356           0 :   return true;
+    4357             : }
+    4358             : 
+    4359             : //}
+    4360             : 
+    4361             : /* //{ callbackToggleOutput() */
+    4362             : 
+    4363          54 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4364             : 
+    4365          54 :   if (!is_initialized_)
+    4366           0 :     return false;
+    4367             : 
+    4368          54 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4369             : 
+    4370             :   // copy member variables
+    4371         108 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4372             : 
+    4373         108 :   std::stringstream ss;
+    4374             : 
+    4375          54 :   bool prereq_check = true;
+    4376             : 
+    4377             :   {
+    4378         108 :     mrs_msgs::ReferenceStamped current_coord;
+    4379          54 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4380          54 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4381          54 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4382             : 
+    4383          54 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4384           0 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4385           0 :       prereq_check = false;
+    4386             :     }
+    4387             :   }
+    4388             : 
+    4389          54 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4390           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4391           0 :     prereq_check = false;
+    4392             :   }
+    4393             : 
+    4394          54 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4395           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4396           0 :     prereq_check = false;
+    4397             :   }
+    4398             : 
+    4399          54 :   if (bumper_enabled_ && !sh_bumper_.hasMsg()) {
+    4400           0 :     ss << "cannot toggle output ON, missing bumper data!";
+    4401           0 :     prereq_check = false;
+    4402             :   }
+    4403             : 
+    4404          54 :   if (!prereq_check) {
+    4405             : 
+    4406           0 :     res.message = ss.str();
+    4407           0 :     res.success = false;
+    4408             : 
+    4409           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4410             : 
+    4411           0 :     return false;
+    4412             : 
+    4413             :   } else {
+    4414             : 
+    4415          54 :     toggleOutput(req.data);
+    4416             : 
+    4417          54 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4418          54 :     res.message = ss.str();
+    4419          54 :     res.success = true;
+    4420             : 
+    4421          54 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4422             : 
+    4423          54 :     publishDiagnostics();
+    4424             : 
+    4425          54 :     return true;
+    4426             :   }
+    4427             : }
+    4428             : 
+    4429             : //}
+    4430             : 
+    4431             : /* callbackArm() //{ */
+    4432             : 
+    4433           3 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4434             : 
+    4435           3 :   if (!is_initialized_)
+    4436           0 :     return false;
+    4437             : 
+    4438           3 :   ROS_INFO("[ControlManager]: arming by service");
+    4439             : 
+    4440           6 :   std::stringstream ss;
+    4441             : 
+    4442           3 :   if (failsafe_triggered_ || eland_triggered_) {
+    4443             : 
+    4444           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4445             : 
+    4446           0 :     res.message = ss.str();
+    4447           0 :     res.success = false;
+    4448             : 
+    4449           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4450             : 
+    4451           0 :     return true;
+    4452             :   }
+    4453             : 
+    4454           3 :   if (req.data) {
+    4455             : 
+    4456           0 :     ss << "this service is not allowed to arm the UAV";
+    4457           0 :     res.success = false;
+    4458           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4459             : 
+    4460             :   } else {
+    4461             : 
+    4462           6 :     auto [success, message] = arming(false);
+    4463             : 
+    4464           3 :     if (success) {
+    4465             : 
+    4466           3 :       ss << "disarmed";
+    4467           3 :       res.success = true;
+    4468           3 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4469             : 
+    4470             :     } else {
+    4471             : 
+    4472           0 :       ss << "could not disarm: " << message;
+    4473           0 :       res.success = false;
+    4474           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4475             :     }
+    4476             :   }
+    4477             : 
+    4478           3 :   res.message = ss.str();
+    4479             : 
+    4480           3 :   return true;
+    4481             : }
+    4482             : 
+    4483             : //}
+    4484             : 
+    4485             : /* //{ callbackEnableCallbacks() */
+    4486             : 
+    4487          84 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4488             : 
+    4489          84 :   if (!is_initialized_)
+    4490           0 :     return false;
+    4491             : 
+    4492          84 :   setCallbacks(req.data);
+    4493             : 
+    4494          84 :   std::stringstream ss;
+    4495             : 
+    4496          84 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4497             : 
+    4498          84 :   res.message = ss.str();
+    4499          84 :   res.success = true;
+    4500             : 
+    4501          84 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4502             : 
+    4503          84 :   return true;
+    4504             : }
+    4505             : 
+    4506             : //}
+    4507             : 
+    4508             : /* callbackSetConstraints() //{ */
+    4509             : 
+    4510          57 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4511             : 
+    4512          57 :   if (!is_initialized_) {
+    4513           0 :     res.message = "not initialized";
+    4514           0 :     res.success = false;
+    4515           0 :     return true;
+    4516             :   }
+    4517             : 
+    4518             :   {
+    4519         114 :     std::scoped_lock lock(mutex_constraints_);
+    4520             : 
+    4521          57 :     current_constraints_ = req;
+    4522             : 
+    4523          57 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4524             : 
+    4525          57 :     if (enforced) {
+    4526           0 :       sanitized_constraints_      = enforced.value();
+    4527           0 :       constraints_being_enforced_ = true;
+    4528             :     } else {
+    4529          57 :       sanitized_constraints_      = req;
+    4530          57 :       constraints_being_enforced_ = false;
+    4531             :     }
+    4532             : 
+    4533          57 :     got_constraints_ = true;
+    4534             : 
+    4535          57 :     setConstraintsToControllers(current_constraints_);
+    4536          57 :     setConstraintsToTrackers(sanitized_constraints_);
+    4537             :   }
+    4538             : 
+    4539          57 :   res.message = "setting constraints";
+    4540          57 :   res.success = true;
+    4541             : 
+    4542          57 :   return true;
+    4543             : }
+    4544             : 
+    4545             : //}
+    4546             : 
+    4547             : /* //{ callbackEmergencyReference() */
+    4548             : 
+    4549          82 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4550             : 
+    4551          82 :   if (!is_initialized_)
+    4552           0 :     return false;
+    4553             : 
+    4554         164 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4555             : 
+    4556          82 :   callbacks_enabled_ = false;
+    4557             : 
+    4558          82 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4559             : 
+    4560         164 :   std::stringstream ss;
+    4561             : 
+    4562             :   // transform the reference to the current frame
+    4563         164 :   mrs_msgs::ReferenceStamped original_reference;
+    4564          82 :   original_reference.header    = req.header;
+    4565          82 :   original_reference.reference = req.reference;
+    4566             : 
+    4567         164 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4568             : 
+    4569          82 :   if (!ret) {
+    4570             : 
+    4571           0 :     ss << "the emergency reference could not be transformed";
+    4572             : 
+    4573           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4574           0 :     res.message = ss.str();
+    4575           0 :     res.success = false;
+    4576           0 :     return true;
+    4577             :   }
+    4578             : 
+    4579          82 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4580             : 
+    4581          82 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4582             : 
+    4583          82 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4584          82 :   req_goto_out.reference = transformed_reference.reference;
+    4585             : 
+    4586             :   {
+    4587         164 :     std::scoped_lock lock(mutex_tracker_list_);
+    4588             : 
+    4589             :     // disable callbacks of all trackers
+    4590          82 :     req_enable_callbacks.data = false;
+    4591         574 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4592         492 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4593             :     }
+    4594             : 
+    4595             :     // enable the callbacks for the active tracker
+    4596          82 :     req_enable_callbacks.data = true;
+    4597          82 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4598             : 
+    4599             :     // call the setReference()
+    4600         246 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    4601         246 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4602             : 
+    4603             :     // disable the callbacks back again
+    4604          82 :     req_enable_callbacks.data = false;
+    4605          82 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4606             : 
+    4607          82 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4608          82 :       res.message = tracker_response->message;
+    4609          82 :       res.success = tracker_response->success;
+    4610             :     } else {
+    4611           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    4612           0 :       res.message = ss.str();
+    4613           0 :       res.success = false;
+    4614             :     }
+    4615             :   }
+    4616             : 
+    4617          82 :   return true;
+    4618             : }
+    4619             : 
+    4620             : //}
+    4621             : 
+    4622             : /* callbackPirouette() //{ */
+    4623             : 
+    4624           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4625             : 
+    4626           0 :   if (!is_initialized_)
+    4627           0 :     return false;
+    4628             : 
+    4629             :   // copy member variables
+    4630           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4631             : 
+    4632             :   double uav_heading;
+    4633             :   try {
+    4634           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4635             :   }
+    4636           0 :   catch (...) {
+    4637           0 :     std::stringstream ss;
+    4638           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4639             : 
+    4640           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4641             : 
+    4642           0 :     res.message = ss.str();
+    4643           0 :     res.success = false;
+    4644             : 
+    4645           0 :     return false;
+    4646             :   }
+    4647             : 
+    4648           0 :   if (_pirouette_enabled_) {
+    4649           0 :     res.success = false;
+    4650           0 :     res.message = "already active";
+    4651           0 :     return true;
+    4652             :   }
+    4653             : 
+    4654           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4655             : 
+    4656           0 :     std::stringstream ss;
+    4657           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4658             : 
+    4659           0 :     res.message = ss.str();
+    4660           0 :     res.success = false;
+    4661             : 
+    4662           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4663             : 
+    4664           0 :     return true;
+    4665             :   }
+    4666             : 
+    4667           0 :   _pirouette_enabled_ = true;
+    4668             : 
+    4669           0 :   setCallbacks(false);
+    4670             : 
+    4671           0 :   pirouette_initial_heading_ = uav_heading;
+    4672           0 :   pirouette_iterator_        = 0;
+    4673           0 :   timer_pirouette_.start();
+    4674             : 
+    4675           0 :   res.success = true;
+    4676           0 :   res.message = "activated";
+    4677             : 
+    4678           0 :   return true;
+    4679             : }
+    4680             : 
+    4681             : //}
+    4682             : 
+    4683             : /* callbackUseJoystick() //{ */
+    4684             : 
+    4685           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4686             : 
+    4687           0 :   if (!is_initialized_) {
+    4688           0 :     return false;
+    4689             :   }
+    4690             : 
+    4691           0 :   std::stringstream ss;
+    4692             : 
+    4693             :   {
+    4694           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4695             : 
+    4696           0 :     if (!success) {
+    4697             : 
+    4698           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4699           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4700             : 
+    4701           0 :       res.success = false;
+    4702           0 :       res.message = ss.str();
+    4703             : 
+    4704           0 :       return true;
+    4705             :     }
+    4706             :   }
+    4707             : 
+    4708           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4709             : 
+    4710           0 :   if (!success) {
+    4711             : 
+    4712           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4713           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4714             : 
+    4715           0 :     res.success = false;
+    4716           0 :     res.message = ss.str();
+    4717             : 
+    4718             :     // switch back to hover tracker
+    4719           0 :     switchTracker(_ehover_tracker_name_);
+    4720             : 
+    4721             :     // switch back to safety controller
+    4722           0 :     switchController(_eland_controller_name_);
+    4723             : 
+    4724           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4725             : 
+    4726           0 :     return true;
+    4727             :   }
+    4728             : 
+    4729           0 :   ss << "switched to joystick control";
+    4730             : 
+    4731           0 :   res.success = true;
+    4732           0 :   res.message = ss.str();
+    4733             : 
+    4734           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4735             : 
+    4736           0 :   return true;
+    4737             : }
+    4738             : 
+    4739             : //}
+    4740             : 
+    4741             : /* //{ callbackHover() */
+    4742             : 
+    4743           0 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4744             : 
+    4745           0 :   if (!is_initialized_)
+    4746           0 :     return false;
+    4747             : 
+    4748           0 :   auto [success, message] = hover();
+    4749             : 
+    4750           0 :   res.success = success;
+    4751           0 :   res.message = message;
+    4752             : 
+    4753           0 :   return true;
+    4754             : }
+    4755             : 
+    4756             : //}
+    4757             : 
+    4758             : /* //{ callbackStartTrajectoryTracking() */
+    4759             : 
+    4760           0 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4761             : 
+    4762           0 :   if (!is_initialized_)
+    4763           0 :     return false;
+    4764             : 
+    4765           0 :   auto [success, message] = startTrajectoryTracking();
+    4766             : 
+    4767           0 :   res.success = success;
+    4768           0 :   res.message = message;
+    4769             : 
+    4770           0 :   return true;
+    4771             : }
+    4772             : 
+    4773             : //}
+    4774             : 
+    4775             : /* //{ callbackStopTrajectoryTracking() */
+    4776             : 
+    4777           0 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4778             : 
+    4779           0 :   if (!is_initialized_)
+    4780           0 :     return false;
+    4781             : 
+    4782           0 :   auto [success, message] = stopTrajectoryTracking();
+    4783             : 
+    4784           0 :   res.success = success;
+    4785           0 :   res.message = message;
+    4786             : 
+    4787           0 :   return true;
+    4788             : }
+    4789             : 
+    4790             : //}
+    4791             : 
+    4792             : /* //{ callbackResumeTrajectoryTracking() */
+    4793             : 
+    4794           0 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4795             : 
+    4796           0 :   if (!is_initialized_)
+    4797           0 :     return false;
+    4798             : 
+    4799           0 :   auto [success, message] = resumeTrajectoryTracking();
+    4800             : 
+    4801           0 :   res.success = success;
+    4802           0 :   res.message = message;
+    4803             : 
+    4804           0 :   return true;
+    4805             : }
+    4806             : 
+    4807             : //}
+    4808             : 
+    4809             : /* //{ callbackGotoTrajectoryStart() */
+    4810             : 
+    4811           0 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4812             : 
+    4813           0 :   if (!is_initialized_)
+    4814           0 :     return false;
+    4815             : 
+    4816           0 :   auto [success, message] = gotoTrajectoryStart();
+    4817             : 
+    4818           0 :   res.success = success;
+    4819           0 :   res.message = message;
+    4820             : 
+    4821           0 :   return true;
+    4822             : }
+    4823             : 
+    4824             : //}
+    4825             : 
+    4826             : /* //{ callbackTransformReference() */
+    4827             : 
+    4828           0 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4829             : 
+    4830           0 :   if (!is_initialized_)
+    4831           0 :     return false;
+    4832             : 
+    4833             :   // transform the reference to the current frame
+    4834           0 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4835             : 
+    4836           0 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4837             : 
+    4838           0 :     res.reference = ret.value();
+    4839           0 :     res.message   = "transformation successful";
+    4840           0 :     res.success   = true;
+    4841           0 :     return true;
+    4842             : 
+    4843             :   } else {
+    4844             : 
+    4845           0 :     res.message = "the reference could not be transformed";
+    4846           0 :     res.success = false;
+    4847           0 :     return true;
+    4848             :   }
+    4849             : 
+    4850             :   return true;
+    4851             : }
+    4852             : 
+    4853             : //}
+    4854             : 
+    4855             : /* //{ callbackTransformPose() */
+    4856             : 
+    4857           0 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    4858             : 
+    4859           0 :   if (!is_initialized_)
+    4860           0 :     return false;
+    4861             : 
+    4862             :   // transform the reference to the current frame
+    4863           0 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    4864             : 
+    4865           0 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    4866             : 
+    4867           0 :     res.pose    = ret.value();
+    4868           0 :     res.message = "transformation successful";
+    4869           0 :     res.success = true;
+    4870           0 :     return true;
+    4871             : 
+    4872             :   } else {
+    4873             : 
+    4874           0 :     res.message = "the pose could not be transformed";
+    4875           0 :     res.success = false;
+    4876           0 :     return true;
+    4877             :   }
+    4878             : 
+    4879             :   return true;
+    4880             : }
+    4881             : 
+    4882             : //}
+    4883             : 
+    4884             : /* //{ callbackTransformVector3() */
+    4885             : 
+    4886           0 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    4887             : 
+    4888           0 :   if (!is_initialized_)
+    4889           0 :     return false;
+    4890             : 
+    4891             :   // transform the reference to the current frame
+    4892           0 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    4893             : 
+    4894           0 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    4895             : 
+    4896           0 :     res.vector  = ret.value();
+    4897           0 :     res.message = "transformation successful";
+    4898           0 :     res.success = true;
+    4899           0 :     return true;
+    4900             : 
+    4901             :   } else {
+    4902             : 
+    4903           0 :     res.message = "the twist could not be transformed";
+    4904           0 :     res.success = false;
+    4905           0 :     return true;
+    4906             :   }
+    4907             : 
+    4908             :   return true;
+    4909             : }
+    4910             : 
+    4911             : //}
+    4912             : 
+    4913             : /* //{ callbackEnableBumper() */
+    4914             : 
+    4915           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4916             : 
+    4917           0 :   if (!is_initialized_)
+    4918           0 :     return false;
+    4919             : 
+    4920           0 :   bumper_enabled_ = req.data;
+    4921             : 
+    4922           0 :   std::stringstream ss;
+    4923             : 
+    4924           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    4925             : 
+    4926           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4927             : 
+    4928           0 :   res.success = true;
+    4929           0 :   res.message = ss.str();
+    4930             : 
+    4931           0 :   return true;
+    4932             : }
+    4933             : 
+    4934             : //}
+    4935             : 
+    4936             : /* //{ callbackUseSafetyArea() */
+    4937             : 
+    4938           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4939             : 
+    4940           0 :   if (!is_initialized_)
+    4941           0 :     return false;
+    4942             : 
+    4943           0 :   use_safety_area_ = req.data;
+    4944             : 
+    4945           0 :   std::stringstream ss;
+    4946             : 
+    4947           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    4948             : 
+    4949           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4950             : 
+    4951           0 :   res.success = true;
+    4952           0 :   res.message = ss.str();
+    4953             : 
+    4954           0 :   return true;
+    4955             : }
+    4956             : 
+    4957             : //}
+    4958             : 
+    4959             : /* //{ callbackGetMinZ() */
+    4960             : 
+    4961           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    4962             : 
+    4963           0 :   if (!is_initialized_) {
+    4964           0 :     return false;
+    4965             :   }
+    4966             : 
+    4967           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4968             : 
+    4969           0 :   res.success = true;
+    4970           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    4971             : 
+    4972           0 :   return true;
+    4973             : }
+    4974             : 
+    4975             : //}
+    4976             : 
+    4977             : /* //{ callbackValidateReference() */
+    4978             : 
+    4979           0 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    4980             : 
+    4981           0 :   if (!is_initialized_) {
+    4982           0 :     res.message = "not initialized";
+    4983           0 :     res.success = false;
+    4984           0 :     return true;
+    4985             :   }
+    4986             : 
+    4987           0 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    4988           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    4989           0 :     res.message = "NaNs/infs in input!";
+    4990           0 :     res.success = false;
+    4991           0 :     return true;
+    4992             :   }
+    4993             : 
+    4994             :   // copy member variables
+    4995           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4996           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    4997             : 
+    4998             :   // transform the reference to the current frame
+    4999           0 :   mrs_msgs::ReferenceStamped original_reference;
+    5000           0 :   original_reference.header    = req.reference.header;
+    5001           0 :   original_reference.reference = req.reference.reference;
+    5002             : 
+    5003           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5004             : 
+    5005           0 :   if (!ret) {
+    5006             : 
+    5007           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5008           0 :     res.message = "the reference could not be transformed";
+    5009           0 :     res.success = false;
+    5010           0 :     return true;
+    5011             :   }
+    5012             : 
+    5013           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5014             : 
+    5015           0 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5016           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5017           0 :     res.message = "the point is outside of the safety area";
+    5018           0 :     res.success = false;
+    5019           0 :     return true;
+    5020             :   }
+    5021             : 
+    5022           0 :   if (last_tracker_cmd) {
+    5023             : 
+    5024           0 :     mrs_msgs::ReferenceStamped from_point;
+    5025           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5026           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5027           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5028           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5029             : 
+    5030           0 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5031           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5032           0 :       res.message = "the path is going outside the safety area";
+    5033           0 :       res.success = false;
+    5034           0 :       return true;
+    5035             :     }
+    5036             :   }
+    5037             : 
+    5038           0 :   res.message = "the reference is ok";
+    5039           0 :   res.success = true;
+    5040           0 :   return true;
+    5041             : }
+    5042             : 
+    5043             : //}
+    5044             : 
+    5045             : /* //{ callbackValidateReference2d() */
+    5046             : 
+    5047        2856 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5048             : 
+    5049        2856 :   if (!is_initialized_) {
+    5050           0 :     res.message = "not initialized";
+    5051           0 :     res.success = false;
+    5052           0 :     return true;
+    5053             :   }
+    5054             : 
+    5055        2856 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5056           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5057           0 :     res.message = "NaNs/infs in input!";
+    5058           0 :     res.success = false;
+    5059           0 :     return true;
+    5060             :   }
+    5061             : 
+    5062             :   // copy member variables
+    5063        5712 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5064        5712 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5065             : 
+    5066             :   // transform the reference to the current frame
+    5067        5712 :   mrs_msgs::ReferenceStamped original_reference;
+    5068        2856 :   original_reference.header    = req.reference.header;
+    5069        2856 :   original_reference.reference = req.reference.reference;
+    5070             : 
+    5071        5712 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5072             : 
+    5073        2856 :   if (!ret) {
+    5074             : 
+    5075           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5076           0 :     res.message = "the reference could not be transformed";
+    5077           0 :     res.success = false;
+    5078           0 :     return true;
+    5079             :   }
+    5080             : 
+    5081        5712 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5082             : 
+    5083        2856 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5084          76 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5085          76 :     res.message = "the point is outside of the safety area";
+    5086          76 :     res.success = false;
+    5087          76 :     return true;
+    5088             :   }
+    5089             : 
+    5090        2780 :   if (last_tracker_cmd) {
+    5091             : 
+    5092          32 :     mrs_msgs::ReferenceStamped from_point;
+    5093          32 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5094          32 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5095          32 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5096          32 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5097             : 
+    5098          32 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5099           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5100           0 :       res.message = "the path is going outside the safety area";
+    5101           0 :       res.success = false;
+    5102           0 :       return true;
+    5103             :     }
+    5104             :   }
+    5105             : 
+    5106        2780 :   res.message = "the reference is ok";
+    5107        2780 :   res.success = true;
+    5108        2780 :   return true;
+    5109             : }
+    5110             : 
+    5111             : //}
+    5112             : 
+    5113             : /* //{ callbackValidateReferenceList() */
+    5114             : 
+    5115           0 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+    5116             : 
+    5117           0 :   if (!is_initialized_) {
+    5118           0 :     res.message = "not initialized";
+    5119           0 :     return false;
+    5120             :   }
+    5121             : 
+    5122             :   // copy member variables
+    5123           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5124           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5125             : 
+    5126             :   // get the transformer
+    5127           0 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+    5128             : 
+    5129           0 :   if (!ret) {
+    5130             : 
+    5131           0 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5132           0 :     res.message = "could not find transform";
+    5133           0 :     return false;
+    5134             :   }
+    5135             : 
+    5136           0 :   geometry_msgs::TransformStamped tf = ret.value();
+    5137             : 
+    5138           0 :   for (int i = 0; i < int(req.list.list.size()); i++) {
+    5139             : 
+    5140           0 :     res.success.push_back(true);
+    5141             : 
+    5142           0 :     mrs_msgs::ReferenceStamped original_reference;
+    5143           0 :     original_reference.header    = req.list.header;
+    5144           0 :     original_reference.reference = req.list.list[i];
+    5145             : 
+    5146           0 :     res.success[i] = validateReference(original_reference.reference, "ControlManager", "reference_list");
+    5147             : 
+    5148           0 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5149             : 
+    5150           0 :     if (!ret) {
+    5151             : 
+    5152           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5153           0 :       res.success[i] = false;
+    5154             :     }
+    5155             : 
+    5156           0 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5157             : 
+    5158           0 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5159           0 :       res.success[i] = false;
+    5160             :     }
+    5161             : 
+    5162           0 :     if (last_tracker_cmd) {
+    5163             : 
+    5164           0 :       mrs_msgs::ReferenceStamped from_point;
+    5165           0 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5166           0 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5167           0 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5168           0 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5169             : 
+    5170           0 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5171           0 :         res.success[i] = false;
+    5172             :       }
+    5173             :     }
+    5174             :   }
+    5175             : 
+    5176           0 :   res.message = "references were checked";
+    5177           0 :   return true;
+    5178             : }
+    5179             : 
+    5180             : //}
+    5181             : 
+    5182             : // | -------------- setpoint topics and services -------------- |
+    5183             : 
+    5184             : /* //{ callbackReferenceService() */
+    5185             : 
+    5186           0 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5187             : 
+    5188           0 :   if (!is_initialized_) {
+    5189           0 :     res.message = "not initialized";
+    5190           0 :     res.success = false;
+    5191           0 :     return true;
+    5192             :   }
+    5193             : 
+    5194           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5195           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5196             : 
+    5197           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5198           0 :   des_reference.header    = req.header;
+    5199           0 :   des_reference.reference = req.reference;
+    5200             : 
+    5201           0 :   auto [success, message] = setReference(des_reference);
+    5202             : 
+    5203           0 :   res.success = success;
+    5204           0 :   res.message = message;
+    5205             : 
+    5206           0 :   return true;
+    5207             : }
+    5208             : 
+    5209             : //}
+    5210             : 
+    5211             : /* //{ callbackReferenceTopic() */
+    5212             : 
+    5213           0 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5214             : 
+    5215           0 :   if (!is_initialized_)
+    5216           0 :     return;
+    5217             : 
+    5218           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5219           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5220             : 
+    5221           0 :   setReference(*msg);
+    5222             : }
+    5223             : 
+    5224             : //}
+    5225             : 
+    5226             : /* //{ callbackVelocityReferenceService() */
+    5227             : 
+    5228         468 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5229             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5230             : 
+    5231         468 :   if (!is_initialized_) {
+    5232           0 :     res.message = "not initialized";
+    5233           0 :     res.success = false;
+    5234           0 :     return true;
+    5235             :   }
+    5236             : 
+    5237        1404 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5238        1404 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5239             : 
+    5240         936 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5241         468 :   des_reference = req.reference;
+    5242             : 
+    5243         468 :   auto [success, message] = setVelocityReference(des_reference);
+    5244             : 
+    5245         468 :   res.success = success;
+    5246         468 :   res.message = message;
+    5247             : 
+    5248         468 :   return true;
+    5249             : }
+    5250             : 
+    5251             : //}
+    5252             : 
+    5253             : /* //{ callbackVelocityReferenceTopic() */
+    5254             : 
+    5255           0 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5256             : 
+    5257           0 :   if (!is_initialized_)
+    5258           0 :     return;
+    5259             : 
+    5260           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5261           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5262             : 
+    5263           0 :   setVelocityReference(*msg);
+    5264             : }
+    5265             : 
+    5266             : //}
+    5267             : 
+    5268             : /* //{ callbackTrajectoryReferenceService() */
+    5269             : 
+    5270           3 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5271             : 
+    5272           3 :   if (!is_initialized_) {
+    5273           0 :     res.message = "not initialized";
+    5274           0 :     res.success = false;
+    5275           0 :     return true;
+    5276             :   }
+    5277             : 
+    5278           9 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5279           9 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5280             : 
+    5281           6 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5282             : 
+    5283           3 :   res.success          = success;
+    5284           3 :   res.message          = message;
+    5285           3 :   res.modified         = modified;
+    5286           3 :   res.tracker_names    = tracker_names;
+    5287           3 :   res.tracker_messages = tracker_messages;
+    5288             : 
+    5289          21 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5290          18 :     res.tracker_successes.push_back(tracker_successes[i]);
+    5291             :   }
+    5292             : 
+    5293           3 :   return true;
+    5294             : }
+    5295             : 
+    5296             : //}
+    5297             : 
+    5298             : /* //{ callbackTrajectoryReferenceTopic() */
+    5299             : 
+    5300           0 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5301             : 
+    5302           0 :   if (!is_initialized_)
+    5303           0 :     return;
+    5304             : 
+    5305           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5306           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5307             : 
+    5308           0 :   setTrajectoryReference(*msg);
+    5309             : }
+    5310             : 
+    5311             : //}
+    5312             : 
+    5313             : // | ------------- human-callable "goto" services ------------- |
+    5314             : 
+    5315             : /* //{ callbackGoto() */
+    5316             : 
+    5317          24 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5318             : 
+    5319          24 :   if (!is_initialized_) {
+    5320           0 :     res.message = "not initialized";
+    5321           0 :     res.success = false;
+    5322           0 :     return true;
+    5323             :   }
+    5324             : 
+    5325          72 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5326          72 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5327             : 
+    5328          48 :   mrs_msgs::ReferenceStamped des_reference;
+    5329          24 :   des_reference.header.frame_id      = "";
+    5330          24 :   des_reference.header.stamp         = ros::Time(0);
+    5331          24 :   des_reference.reference.position.x = req.goal[REF_X];
+    5332          24 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5333          24 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5334          24 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5335             : 
+    5336          48 :   auto [success, message] = setReference(des_reference);
+    5337             : 
+    5338          24 :   res.success = success;
+    5339          24 :   res.message = message;
+    5340             : 
+    5341          24 :   return true;
+    5342             : }
+    5343             : 
+    5344             : //}
+    5345             : 
+    5346             : /* //{ callbackGotoFcu() */
+    5347             : 
+    5348           0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5349             : 
+    5350           0 :   if (!is_initialized_) {
+    5351           0 :     res.message = "not initialized";
+    5352           0 :     res.success = false;
+    5353           0 :     return true;
+    5354             :   }
+    5355             : 
+    5356           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5357           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5358             : 
+    5359           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5360           0 :   des_reference.header.frame_id      = "fcu_untilted";
+    5361           0 :   des_reference.header.stamp         = ros::Time(0);
+    5362           0 :   des_reference.reference.position.x = req.goal[REF_X];
+    5363           0 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5364           0 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5365           0 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5366             : 
+    5367           0 :   auto [success, message] = setReference(des_reference);
+    5368             : 
+    5369           0 :   res.success = success;
+    5370           0 :   res.message = message;
+    5371             : 
+    5372           0 :   return true;
+    5373             : }
+    5374             : 
+    5375             : //}
+    5376             : 
+    5377             : /* //{ callbackGotoRelative() */
+    5378             : 
+    5379          16 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5380             : 
+    5381          16 :   if (!is_initialized_) {
+    5382           0 :     res.message = "not initialized";
+    5383           0 :     res.success = false;
+    5384           0 :     return true;
+    5385             :   }
+    5386             : 
+    5387          48 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5388          48 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5389             : 
+    5390          32 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5391             : 
+    5392          16 :   if (!last_tracker_cmd) {
+    5393           0 :     res.message = "not flying";
+    5394           0 :     res.success = false;
+    5395           0 :     return true;
+    5396             :   }
+    5397             : 
+    5398          32 :   mrs_msgs::ReferenceStamped des_reference;
+    5399          16 :   des_reference.header.frame_id      = "";
+    5400          16 :   des_reference.header.stamp         = ros::Time(0);
+    5401          16 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal[REF_X];
+    5402          16 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal[REF_Y];
+    5403          16 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal[REF_Z];
+    5404          16 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal[REF_HEADING];
+    5405             : 
+    5406          32 :   auto [success, message] = setReference(des_reference);
+    5407             : 
+    5408          16 :   res.success = success;
+    5409          16 :   res.message = message;
+    5410             : 
+    5411          16 :   return true;
+    5412             : }
+    5413             : 
+    5414             : //}
+    5415             : 
+    5416             : /* //{ callbackGotoAltitude() */
+    5417             : 
+    5418           0 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5419             : 
+    5420           0 :   if (!is_initialized_) {
+    5421           0 :     res.message = "not initialized";
+    5422           0 :     res.success = false;
+    5423           0 :     return true;
+    5424             :   }
+    5425             : 
+    5426           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5427           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5428             : 
+    5429           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5430             : 
+    5431           0 :   if (!last_tracker_cmd) {
+    5432           0 :     res.message = "not flying";
+    5433           0 :     res.success = false;
+    5434           0 :     return true;
+    5435             :   }
+    5436             : 
+    5437           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5438           0 :   des_reference.header.frame_id      = "";
+    5439           0 :   des_reference.header.stamp         = ros::Time(0);
+    5440           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5441           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5442           0 :   des_reference.reference.position.z = req.goal;
+    5443           0 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5444             : 
+    5445           0 :   auto [success, message] = setReference(des_reference);
+    5446             : 
+    5447           0 :   res.success = success;
+    5448           0 :   res.message = message;
+    5449             : 
+    5450           0 :   return true;
+    5451             : }
+    5452             : 
+    5453             : //}
+    5454             : 
+    5455             : /* //{ callbackSetHeading() */
+    5456             : 
+    5457           0 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5458             : 
+    5459           0 :   if (!is_initialized_) {
+    5460           0 :     res.message = "not initialized";
+    5461           0 :     res.success = false;
+    5462           0 :     return true;
+    5463             :   }
+    5464             : 
+    5465           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5466           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5467             : 
+    5468           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5469             : 
+    5470           0 :   if (!last_tracker_cmd) {
+    5471           0 :     res.message = "not flying";
+    5472           0 :     res.success = false;
+    5473           0 :     return true;
+    5474             :   }
+    5475             : 
+    5476           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5477           0 :   des_reference.header.frame_id      = "";
+    5478           0 :   des_reference.header.stamp         = ros::Time(0);
+    5479           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5480           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5481           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5482           0 :   des_reference.reference.heading    = req.goal;
+    5483             : 
+    5484           0 :   auto [success, message] = setReference(des_reference);
+    5485             : 
+    5486           0 :   res.success = success;
+    5487           0 :   res.message = message;
+    5488             : 
+    5489           0 :   return true;
+    5490             : }
+    5491             : 
+    5492             : //}
+    5493             : 
+    5494             : /* //{ callbackSetHeadingRelative() */
+    5495             : 
+    5496           0 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5497             : 
+    5498           0 :   if (!is_initialized_) {
+    5499           0 :     res.message = "not initialized";
+    5500           0 :     res.success = false;
+    5501           0 :     return true;
+    5502             :   }
+    5503             : 
+    5504           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5505           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5506             : 
+    5507           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5508             : 
+    5509           0 :   if (!last_tracker_cmd) {
+    5510           0 :     res.message = "not flying";
+    5511           0 :     res.success = false;
+    5512           0 :     return true;
+    5513             :   }
+    5514             : 
+    5515           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5516           0 :   des_reference.header.frame_id      = "";
+    5517           0 :   des_reference.header.stamp         = ros::Time(0);
+    5518           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5519           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5520           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5521           0 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5522             : 
+    5523           0 :   auto [success, message] = setReference(des_reference);
+    5524             : 
+    5525           0 :   res.success = success;
+    5526           0 :   res.message = message;
+    5527             : 
+    5528           0 :   return true;
+    5529             : }
+    5530             : 
+    5531             : //}
+    5532             : 
+    5533             : // --------------------------------------------------------------
+    5534             : // |                          routines                          |
+    5535             : // --------------------------------------------------------------
+    5536             : 
+    5537             : /* setReference() //{ */
+    5538             : 
+    5539          40 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5540             : 
+    5541          80 :   std::stringstream ss;
+    5542             : 
+    5543          40 :   if (!callbacks_enabled_) {
+    5544           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5545           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5546           0 :     return std::tuple(false, ss.str());
+    5547             :   }
+    5548             : 
+    5549          40 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5550           0 :     ss << "incoming reference is not finite!!!";
+    5551           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5552           0 :     return std::tuple(false, ss.str());
+    5553             :   }
+    5554             : 
+    5555             :   // copy member variables
+    5556          80 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5557          80 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5558             : 
+    5559             :   // transform the reference to the current frame
+    5560          80 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5561             : 
+    5562          40 :   if (!ret) {
+    5563             : 
+    5564           0 :     ss << "the reference could not be transformed";
+    5565           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5566           0 :     return std::tuple(false, ss.str());
+    5567             :   }
+    5568             : 
+    5569          80 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5570             : 
+    5571             :   // safety area check
+    5572          40 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5573           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5574           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5575           0 :     return std::tuple(false, ss.str());
+    5576             :   }
+    5577             : 
+    5578          40 :   if (last_tracker_cmd) {
+    5579             : 
+    5580          40 :     mrs_msgs::ReferenceStamped from_point;
+    5581          40 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5582          40 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5583          40 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5584          40 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5585             : 
+    5586          40 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5587           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5588           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5589           0 :       return std::tuple(false, ss.str());
+    5590             :     }
+    5591             :   }
+    5592             : 
+    5593          40 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5594             : 
+    5595             :   // prepare the message for current tracker
+    5596          40 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5597          40 :   reference_request.reference = transformed_reference.reference;
+    5598             : 
+    5599             :   {
+    5600          80 :     std::scoped_lock lock(mutex_tracker_list_);
+    5601             : 
+    5602         120 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    5603         120 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5604             : 
+    5605          40 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5606             : 
+    5607          80 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5608             : 
+    5609             :     } else {
+    5610             : 
+    5611           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    5612           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5613           0 :       return std::tuple(false, ss.str());
+    5614             :     }
+    5615             :   }
+    5616             : }
+    5617             : 
+    5618             : //}
+    5619             : 
+    5620             : /* setVelocityReference() //{ */
+    5621             : 
+    5622         468 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5623             : 
+    5624         936 :   std::stringstream ss;
+    5625             : 
+    5626         468 :   if (!callbacks_enabled_) {
+    5627           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5628           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5629           0 :     return std::tuple(false, ss.str());
+    5630             :   }
+    5631             : 
+    5632         468 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5633           0 :     ss << "velocity command is not valid!";
+    5634           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5635           0 :     return std::tuple(false, ss.str());
+    5636             :   }
+    5637             : 
+    5638             :   {
+    5639         468 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5640             : 
+    5641         468 :     if (!last_tracker_cmd_) {
+    5642           0 :       ss << "could not set velocity command, not flying!";
+    5643           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5644           0 :       return std::tuple(false, ss.str());
+    5645             :     }
+    5646             :   }
+    5647             : 
+    5648             :   // copy member variables
+    5649         936 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5650         936 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5651             : 
+    5652             :   // | -- transform the velocity reference to the current frame - |
+    5653             : 
+    5654         936 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5655             : 
+    5656         936 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5657             : 
+    5658         936 :   geometry_msgs::TransformStamped tf;
+    5659             : 
+    5660         468 :   if (!ret) {
+    5661           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5662           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5663           0 :     return std::tuple(false, ss.str());
+    5664             :   } else {
+    5665         468 :     tf = ret.value();
+    5666             :   }
+    5667             : 
+    5668             :   // transform the velocity
+    5669             :   {
+    5670         468 :     geometry_msgs::Vector3Stamped velocity;
+    5671         468 :     velocity.header   = reference_in.header;
+    5672         468 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5673         468 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5674         468 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5675             : 
+    5676         468 :     auto ret = transformer_->transform(velocity, tf);
+    5677             : 
+    5678         468 :     if (!ret) {
+    5679             : 
+    5680           0 :       ss << "the velocity reference could not be transformed";
+    5681           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5682           0 :       return std::tuple(false, ss.str());
+    5683             : 
+    5684             :     } else {
+    5685         468 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5686         468 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5687         468 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5688             :     }
+    5689             :   }
+    5690             : 
+    5691             :   // transform the z and the heading
+    5692             :   {
+    5693         468 :     geometry_msgs::PoseStamped pose;
+    5694         468 :     pose.header           = reference_in.header;
+    5695         468 :     pose.pose.position.x  = 0;
+    5696         468 :     pose.pose.position.y  = 0;
+    5697         468 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5698         468 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5699             : 
+    5700         468 :     auto ret = transformer_->transform(pose, tf);
+    5701             : 
+    5702         468 :     if (!ret) {
+    5703             : 
+    5704           0 :       ss << "the velocity reference could not be transformed";
+    5705           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5706           0 :       return std::tuple(false, ss.str());
+    5707             : 
+    5708             :     } else {
+    5709         468 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5710         468 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5711             :     }
+    5712             :   }
+    5713             : 
+    5714             :   // the heading rate doees not need to be transformed
+    5715         468 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5716             : 
+    5717         468 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5718         468 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5719             : 
+    5720         936 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5721             : 
+    5722         468 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5723             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5724             : 
+    5725             :   // safety area check
+    5726         468 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5727           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5728           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5729           0 :     return std::tuple(false, ss.str());
+    5730             :   }
+    5731             : 
+    5732         468 :   if (last_tracker_cmd) {
+    5733             : 
+    5734         468 :     mrs_msgs::ReferenceStamped from_point;
+    5735         468 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5736         468 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5737         468 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5738         468 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5739             : 
+    5740         468 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5741           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5742           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5743           0 :       return std::tuple(false, ss.str());
+    5744             :     }
+    5745             :   }
+    5746             : 
+    5747         468 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5748             : 
+    5749             :   // prepare the message for current tracker
+    5750         468 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5751         468 :   reference_request.reference = transformed_reference.reference;
+    5752             : 
+    5753             :   {
+    5754         936 :     std::scoped_lock lock(mutex_tracker_list_);
+    5755             : 
+    5756        1404 :     tracker_response = tracker_list_[active_tracker_idx_]->setVelocityReference(
+    5757        1404 :         mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5758             : 
+    5759         468 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5760             : 
+    5761         936 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5762             : 
+    5763             :     } else {
+    5764             : 
+    5765           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setVelocityReference()' function!";
+    5766           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5767           0 :       return std::tuple(false, ss.str());
+    5768             :     }
+    5769             :   }
+    5770             : }
+    5771             : 
+    5772             : //}
+    5773             : 
+    5774             : /* setTrajectoryReference() //{ */
+    5775             : 
+    5776           3 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5777             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5778             : 
+    5779           6 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5780           6 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5781             : 
+    5782           6 :   std::stringstream ss;
+    5783             : 
+    5784           3 :   if (!callbacks_enabled_) {
+    5785           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5786           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5787           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5788             :   }
+    5789             : 
+    5790             :   /* validate the size and check for NaNs //{ */
+    5791             : 
+    5792             :   // check for the size 0, which is invalid
+    5793           3 :   if (trajectory_in.points.size() == 0) {
+    5794             : 
+    5795           0 :     ss << "can not load trajectory with size 0";
+    5796           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5797           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5798             :   }
+    5799             : 
+    5800         418 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5801             : 
+    5802             :     // check the point for NaN/inf
+    5803         415 :     bool valid = validateReference(trajectory_in.points[i], "ControlManager", "trajectory_in.points[]");
+    5804             : 
+    5805         415 :     if (!valid) {
+    5806             : 
+    5807           0 :       ss << "trajectory contains NaNs/infs.";
+    5808           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5809           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5810             :     }
+    5811             :   }
+    5812             : 
+    5813             :   //}
+    5814             : 
+    5815             :   /* publish the debugging topics of the original trajectory //{ */
+    5816             : 
+    5817             :   {
+    5818             : 
+    5819           6 :     geometry_msgs::PoseArray debug_trajectory_out;
+    5820           3 :     debug_trajectory_out.header = trajectory_in.header;
+    5821             : 
+    5822           3 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    5823             : 
+    5824           3 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    5825           0 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    5826             :     }
+    5827             : 
+    5828         415 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5829             : 
+    5830         412 :       geometry_msgs::Pose new_pose;
+    5831             : 
+    5832         412 :       new_pose.position.x = trajectory_in.points[i].position.x;
+    5833         412 :       new_pose.position.y = trajectory_in.points[i].position.y;
+    5834         412 :       new_pose.position.z = trajectory_in.points[i].position.z;
+    5835             : 
+    5836         412 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points[i].heading);
+    5837             : 
+    5838         412 :       debug_trajectory_out.poses.push_back(new_pose);
+    5839             :     }
+    5840             : 
+    5841           3 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    5842             : 
+    5843           6 :     visualization_msgs::MarkerArray msg_out;
+    5844             : 
+    5845           6 :     visualization_msgs::Marker marker;
+    5846             : 
+    5847           3 :     marker.header = trajectory_in.header;
+    5848             : 
+    5849           3 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    5850             : 
+    5851           3 :     if (marker.header.frame_id == "") {
+    5852           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    5853             :     }
+    5854             : 
+    5855           3 :     if (marker.header.stamp == ros::Time(0)) {
+    5856           0 :       marker.header.stamp = ros::Time::now();
+    5857             :     }
+    5858             : 
+    5859           3 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    5860           3 :     marker.color.a          = 1;
+    5861           3 :     marker.scale.x          = 0.05;
+    5862           3 :     marker.color.r          = 0;
+    5863           3 :     marker.color.g          = 1;
+    5864           3 :     marker.color.b          = 0;
+    5865           3 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    5866             : 
+    5867         415 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5868             : 
+    5869         412 :       geometry_msgs::Point point1;
+    5870             : 
+    5871         412 :       point1.x = trajectory_in.points[i].position.x;
+    5872         412 :       point1.y = trajectory_in.points[i].position.y;
+    5873         412 :       point1.z = trajectory_in.points[i].position.z;
+    5874             : 
+    5875         412 :       marker.points.push_back(point1);
+    5876             : 
+    5877         412 :       geometry_msgs::Point point2;
+    5878             : 
+    5879         412 :       point2.x = trajectory_in.points[i + 1].position.x;
+    5880         412 :       point2.y = trajectory_in.points[i + 1].position.y;
+    5881         412 :       point2.z = trajectory_in.points[i + 1].position.z;
+    5882             : 
+    5883         412 :       marker.points.push_back(point2);
+    5884             :     }
+    5885             : 
+    5886           3 :     msg_out.markers.push_back(marker);
+    5887             : 
+    5888           3 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    5889             :   }
+    5890             : 
+    5891             :   //}
+    5892             : 
+    5893           6 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    5894             : 
+    5895           3 :   int trajectory_size = int(processed_trajectory.points.size());
+    5896             : 
+    5897           3 :   bool trajectory_modified = false;
+    5898             : 
+    5899             :   /* safety area check //{ */
+    5900             : 
+    5901           3 :   if (use_safety_area_) {
+    5902             : 
+    5903           3 :     int last_valid_idx    = 0;
+    5904           3 :     int first_invalid_idx = -1;
+    5905             : 
+    5906           3 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    5907           3 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    5908             : 
+    5909         418 :     for (int i = 0; i < trajectory_size; i++) {
+    5910             : 
+    5911         415 :       if (_snap_trajectory_to_safety_area_) {
+    5912             : 
+    5913             :         // saturate the trajectory to min and max Z
+    5914           0 :         if (processed_trajectory.points[i].position.z < min_z) {
+    5915             : 
+    5916           0 :           processed_trajectory.points[i].position.z = min_z;
+    5917           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    5918           0 :           trajectory_modified = true;
+    5919             :         }
+    5920             : 
+    5921           0 :         if (processed_trajectory.points[i].position.z > max_z) {
+    5922             : 
+    5923           0 :           processed_trajectory.points[i].position.z = max_z;
+    5924           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    5925           0 :           trajectory_modified = true;
+    5926             :         }
+    5927             :       }
+    5928             : 
+    5929             :       // check the point against the safety area
+    5930         415 :       mrs_msgs::ReferenceStamped des_reference;
+    5931         415 :       des_reference.header    = processed_trajectory.header;
+    5932         415 :       des_reference.reference = processed_trajectory.points[i];
+    5933             : 
+    5934         415 :       if (!isPointInSafetyArea3d(des_reference)) {
+    5935             : 
+    5936           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    5937           0 :         trajectory_modified = true;
+    5938             : 
+    5939             :         // the first invalid point
+    5940           0 :         if (first_invalid_idx == -1) {
+    5941             : 
+    5942           0 :           first_invalid_idx = i;
+    5943             : 
+    5944           0 :           last_valid_idx = i - 1;
+    5945             :         }
+    5946             : 
+    5947             :         // the point is ok
+    5948             :       } else {
+    5949             : 
+    5950             :         // we found a point, which is ok, after finding a point which was not ok
+    5951         415 :         if (first_invalid_idx != -1) {
+    5952             : 
+    5953             :           // special case, we had no valid point so far
+    5954           0 :           if (last_valid_idx == -1) {
+    5955             : 
+    5956           0 :             ss << "the trajectory starts outside of the safety area!";
+    5957           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5958           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5959             : 
+    5960             :             // we have a valid point in the past
+    5961             :           } else {
+    5962             : 
+    5963           0 :             if (!_snap_trajectory_to_safety_area_) {
+    5964           0 :               break;
+    5965             :             }
+    5966             : 
+    5967           0 :             bool interpolation_success = true;
+    5968             : 
+    5969             :             // iterpolate between the last valid point and this new valid point
+    5970           0 :             double angle = atan2((processed_trajectory.points[i].position.y - processed_trajectory.points[last_valid_idx].position.y),
+    5971           0 :                                  (processed_trajectory.points[i].position.x - processed_trajectory.points[last_valid_idx].position.x));
+    5972             : 
+    5973             :             double dist_two_points =
+    5974           0 :                 mrs_lib::geometry::dist(vec2_t(processed_trajectory.points[i].position.x, processed_trajectory.points[i].position.y),
+    5975           0 :                                         vec2_t(processed_trajectory.points[last_valid_idx].position.x, processed_trajectory.points[last_valid_idx].position.y));
+    5976           0 :             double step = dist_two_points / (i - last_valid_idx);
+    5977             : 
+    5978           0 :             for (int j = last_valid_idx; j < i; j++) {
+    5979             : 
+    5980           0 :               mrs_msgs::ReferenceStamped temp_point;
+    5981           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    5982           0 :               temp_point.reference.position.x = processed_trajectory.points[last_valid_idx].position.x + (j - last_valid_idx) * cos(angle) * step;
+    5983           0 :               temp_point.reference.position.y = processed_trajectory.points[last_valid_idx].position.y + (j - last_valid_idx) * sin(angle) * step;
+    5984             : 
+    5985           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    5986             : 
+    5987           0 :                 interpolation_success = false;
+    5988           0 :                 break;
+    5989             : 
+    5990             :               } else {
+    5991             : 
+    5992           0 :                 processed_trajectory.points[j].position.x = temp_point.reference.position.x;
+    5993           0 :                 processed_trajectory.points[j].position.y = temp_point.reference.position.y;
+    5994             :               }
+    5995             :             }
+    5996             : 
+    5997           0 :             if (!interpolation_success) {
+    5998           0 :               break;
+    5999             :             }
+    6000             :           }
+    6001             : 
+    6002           0 :           first_invalid_idx = -1;
+    6003             :         }
+    6004             :       }
+    6005             :     }
+    6006             : 
+    6007             :     // special case, the trajectory does not end with a valid point
+    6008           3 :     if (first_invalid_idx != -1) {
+    6009             : 
+    6010             :       // super special case, the whole trajectory is invalid
+    6011           0 :       if (first_invalid_idx == 0) {
+    6012             : 
+    6013           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6014           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6015           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6016             : 
+    6017             :         // there is a good portion of the trajectory in the beginning
+    6018             :       } else {
+    6019             : 
+    6020           0 :         trajectory_size = last_valid_idx + 1;
+    6021           0 :         processed_trajectory.points.resize(trajectory_size);
+    6022           0 :         trajectory_modified = true;
+    6023             :       }
+    6024             :     }
+    6025             :   }
+    6026             : 
+    6027           3 :   if (trajectory_size == 0) {
+    6028             : 
+    6029           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6030           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6031           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6032             :   }
+    6033             : 
+    6034             :   //}
+    6035             : 
+    6036             :   /* transform the trajectory to the current control frame //{ */
+    6037             : 
+    6038           3 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6039             : 
+    6040           3 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6041           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6042             :   } else {
+    6043           3 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6044             :   }
+    6045             : 
+    6046           3 :   if (!tf_traj_state) {
+    6047           0 :     ss << "could not create TF transformer for the trajectory";
+    6048           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6049           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6050             :   }
+    6051             : 
+    6052           3 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6053             : 
+    6054         418 :   for (int i = 0; i < trajectory_size; i++) {
+    6055             : 
+    6056         415 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6057         415 :     trajectory_point.header    = processed_trajectory.header;
+    6058         415 :     trajectory_point.reference = processed_trajectory.points[i];
+    6059             : 
+    6060         415 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6061             : 
+    6062         415 :     if (!ret) {
+    6063             : 
+    6064           0 :       ss << "trajectory cannnot be transformed";
+    6065           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6066           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6067             : 
+    6068             :     } else {
+    6069             : 
+    6070             :       // transform the points in the trajectory to the current frame
+    6071         415 :       processed_trajectory.points[i] = ret.value().reference;
+    6072             :     }
+    6073             :   }
+    6074             : 
+    6075             :   //}
+    6076             : 
+    6077           3 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6078           6 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6079             : 
+    6080             :   // check for empty trajectory
+    6081           3 :   if (processed_trajectory.points.size() == 0) {
+    6082           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6083           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6084           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6085             :   }
+    6086             : 
+    6087             :   // prepare the message for current tracker
+    6088           3 :   request.trajectory = processed_trajectory;
+    6089             : 
+    6090             :   bool                     success;
+    6091           6 :   std::string              message;
+    6092             :   bool                     modified;
+    6093           6 :   std::vector<std::string> tracker_names;
+    6094           6 :   std::vector<bool>        tracker_successes;
+    6095           6 :   std::vector<std::string> tracker_messages;
+    6096             : 
+    6097             :   {
+    6098           6 :     std::scoped_lock lock(mutex_tracker_list_);
+    6099             : 
+    6100             :     // set the trajectory to the currently active tracker
+    6101           9 :     response = tracker_list_[active_tracker_idx_]->setTrajectoryReference(
+    6102           9 :         mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6103             : 
+    6104           3 :     tracker_names.push_back(_tracker_names_[active_tracker_idx_]);
+    6105             : 
+    6106           3 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6107             : 
+    6108           3 :       success  = response->success;
+    6109           3 :       message  = response->message;
+    6110           3 :       modified = response->modified || trajectory_modified;
+    6111           3 :       tracker_successes.push_back(response->success);
+    6112           3 :       tracker_messages.push_back(response->message);
+    6113             : 
+    6114             :     } else {
+    6115             : 
+    6116           0 :       ss << "the active tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setTrajectoryReference()' function!";
+    6117           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the trajectory: " << ss.str());
+    6118             : 
+    6119           0 :       success  = false;
+    6120           0 :       message  = ss.str();
+    6121           0 :       modified = false;
+    6122           0 :       tracker_successes.push_back(false);
+    6123           0 :       tracker_messages.push_back(ss.str());
+    6124             :     }
+    6125             : 
+    6126             :     // set the trajectory to the non-active trackers
+    6127          21 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6128             : 
+    6129          18 :       if (i != active_tracker_idx_) {
+    6130             : 
+    6131          15 :         tracker_names.push_back(_tracker_names_[i]);
+    6132             : 
+    6133          45 :         response = tracker_list_[i]->setTrajectoryReference(
+    6134          45 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6135             : 
+    6136          15 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6137             : 
+    6138           0 :           tracker_successes.push_back(response->success);
+    6139           0 :           tracker_messages.push_back(response->message);
+    6140             : 
+    6141           0 :           if (response->success) {
+    6142           0 :             std::stringstream ss;
+    6143           0 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_[i];
+    6144           0 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6145             :           }
+    6146             : 
+    6147             :         } else {
+    6148             : 
+    6149          15 :           std::stringstream ss;
+    6150          15 :           ss << "the tracker \"" << _tracker_names_[i] << "\" does not implement setTrajectoryReference()";
+    6151          15 :           tracker_successes.push_back(false);
+    6152          15 :           tracker_messages.push_back(ss.str());
+    6153             :         }
+    6154             :       }
+    6155             :     }
+    6156             :   }
+    6157             : 
+    6158           3 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6159             : }
+    6160             : 
+    6161             : //}
+    6162             : 
+    6163             : /* isOffboard() //{ */
+    6164             : 
+    6165          13 : bool ControlManager::isOffboard(void) {
+    6166             : 
+    6167          13 :   if (!sh_hw_api_status_.hasMsg()) {
+    6168           0 :     return false;
+    6169             :   }
+    6170             : 
+    6171          13 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6172             : 
+    6173          13 :   return hw_api_status->connected && hw_api_status->offboard;
+    6174             : }
+    6175             : 
+    6176             : //}
+    6177             : 
+    6178             : /* setCallbacks() //{ */
+    6179             : 
+    6180          84 : void ControlManager::setCallbacks(bool in) {
+    6181             : 
+    6182          84 :   callbacks_enabled_ = in;
+    6183             : 
+    6184          84 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6185          84 :   req_enable_callbacks.data = callbacks_enabled_;
+    6186             : 
+    6187             :   {
+    6188         168 :     std::scoped_lock lock(mutex_tracker_list_);
+    6189             : 
+    6190             :     // set callbacks to all trackers
+    6191         588 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6192         504 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6193             :     }
+    6194             :   }
+    6195          84 : }
+    6196             : 
+    6197             : //}
+    6198             : 
+    6199             : /* publishDiagnostics() //{ */
+    6200             : 
+    6201        9599 : void ControlManager::publishDiagnostics(void) {
+    6202             : 
+    6203        9599 :   if (!is_initialized_) {
+    6204           0 :     return;
+    6205             :   }
+    6206             : 
+    6207       28797 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6208       28797 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6209             : 
+    6210       19198 :   std::scoped_lock lock(mutex_diagnostics_);
+    6211             : 
+    6212       19198 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6213             : 
+    6214        9599 :   diagnostics_msg.stamp    = ros::Time::now();
+    6215        9599 :   diagnostics_msg.uav_name = _uav_name_;
+    6216             : 
+    6217        9599 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6218             : 
+    6219        9599 :   diagnostics_msg.output_enabled = output_enabled_;
+    6220             : 
+    6221        9599 :   diagnostics_msg.rc_mode = rc_goto_active_;
+    6222             : 
+    6223             :   {
+    6224        9599 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6225             : 
+    6226        9599 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6227             :   }
+    6228             : 
+    6229             :   // | ----------------- fill the tracker status ---------------- |
+    6230             : 
+    6231             :   {
+    6232       19198 :     std::scoped_lock lock(mutex_tracker_list_);
+    6233             : 
+    6234        9599 :     mrs_msgs::TrackerStatus tracker_status;
+    6235             : 
+    6236        9599 :     diagnostics_msg.active_tracker = _tracker_names_[active_tracker_idx_];
+    6237        9599 :     diagnostics_msg.tracker_status = tracker_list_[active_tracker_idx_]->getStatus();
+    6238             :   }
+    6239             : 
+    6240             :   // | --------------- fill the controller status --------------- |
+    6241             : 
+    6242             :   {
+    6243       19198 :     std::scoped_lock lock(mutex_controller_list_);
+    6244             : 
+    6245        9599 :     mrs_msgs::ControllerStatus controller_status;
+    6246             : 
+    6247        9599 :     diagnostics_msg.active_controller = _controller_names_[active_controller_idx_];
+    6248        9599 :     diagnostics_msg.controller_status = controller_list_[active_controller_idx_]->getStatus();
+    6249             :   }
+    6250             : 
+    6251             :   // | ------------ fill in the available controllers ----------- |
+    6252             : 
+    6253       57594 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6254       47995 :     if ((_controller_names_[i] != _failsafe_controller_name_) && (_controller_names_[i] != _eland_controller_name_)) {
+    6255       28797 :       diagnostics_msg.available_controllers.push_back(_controller_names_[i]);
+    6256       28797 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_[i]).human_switchable);
+    6257             :     }
+    6258             :   }
+    6259             : 
+    6260             :   // | ------------- fill in the available trackers ------------- |
+    6261             : 
+    6262       67413 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6263       57814 :     if (_tracker_names_[i] != _null_tracker_name_) {
+    6264       48215 :       diagnostics_msg.available_trackers.push_back(_tracker_names_[i]);
+    6265       48215 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_[i]).human_switchable);
+    6266             :     }
+    6267             :   }
+    6268             : 
+    6269             :   // | ------------------------- publish ------------------------ |
+    6270             : 
+    6271        9599 :   ph_diagnostics_.publish(diagnostics_msg);
+    6272             : }
+    6273             : 
+    6274             : //}
+    6275             : 
+    6276             : /* setConstraintsToTrackers() //{ */
+    6277             : 
+    6278         182 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6279             : 
+    6280         546 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6281         546 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6282             : 
+    6283         182 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6284             : 
+    6285             :   {
+    6286         364 :     std::scoped_lock lock(mutex_tracker_list_);
+    6287             : 
+    6288             :     // for each tracker
+    6289        1277 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6290             : 
+    6291             :       // if it is the active one, update and retrieve the command
+    6292        3285 :       response = tracker_list_[i]->setConstraints(
+    6293        3285 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6294             :     }
+    6295             :   }
+    6296         182 : }
+    6297             : 
+    6298             : //}
+    6299             : 
+    6300             : /* setConstraintsToControllers() //{ */
+    6301             : 
+    6302         227 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6303             : 
+    6304         681 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6305         681 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6306             : 
+    6307         227 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6308             : 
+    6309             :   {
+    6310         454 :     std::scoped_lock lock(mutex_controller_list_);
+    6311             : 
+    6312             :     // for each controller
+    6313        1362 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6314             : 
+    6315             :       // if it is the active one, update and retrieve the command
+    6316        3405 :       response = controller_list_[i]->setConstraints(
+    6317        3405 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6318             :     }
+    6319             :   }
+    6320         227 : }
+    6321             : 
+    6322             : //}
+    6323             : 
+    6324             : /* setConstraints() //{ */
+    6325             : 
+    6326          57 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6327             : 
+    6328         171 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6329         171 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6330             : 
+    6331          57 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6332             : 
+    6333          57 :   setConstraintsToTrackers(constraints);
+    6334             : 
+    6335          57 :   setConstraintsToControllers(constraints);
+    6336          57 : }
+    6337             : 
+    6338             : //}
+    6339             : 
+    6340             : 
+    6341             : /* enforceControllerConstraints() //{ */
+    6342             : 
+    6343       64266 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6344             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6345             : 
+    6346             :   // copy member variables
+    6347      128532 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6348             : 
+    6349       64266 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6350       52613 :     return {};
+    6351             :   }
+    6352             : 
+    6353       11653 :   bool enforcing = false;
+    6354             : 
+    6355       11653 :   auto constraints_out = constraints;
+    6356             : 
+    6357       23306 :   std::scoped_lock lock(mutex_tracker_list_);
+    6358             : 
+    6359             :   // enforce horizontal speed
+    6360       11653 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6361        8474 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6362             : 
+    6363        8474 :     enforcing = true;
+    6364             :   }
+    6365             : 
+    6366             :   // enforce horizontal acceleration
+    6367       11653 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6368       11006 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6369             : 
+    6370       11006 :     enforcing = true;
+    6371             :   }
+    6372             : 
+    6373             :   // enforce vertical ascending speed
+    6374       11653 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6375        8474 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6376             : 
+    6377        8474 :     enforcing = true;
+    6378             :   }
+    6379             : 
+    6380             :   // enforce vertical ascending acceleration
+    6381       11653 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6382           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6383             : 
+    6384           0 :     enforcing = true;
+    6385             :   }
+    6386             : 
+    6387             :   // enforce vertical descending speed
+    6388       11653 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6389        8474 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6390             : 
+    6391        8474 :     enforcing = true;
+    6392             :   }
+    6393             : 
+    6394             :   // enforce vertical descending acceleration
+    6395       11653 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6396           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6397             : 
+    6398           0 :     enforcing = true;
+    6399             :   }
+    6400             : 
+    6401       11653 :   if (enforcing) {
+    6402       11006 :     return {constraints_out};
+    6403             :   } else {
+    6404         647 :     return {};
+    6405             :   }
+    6406             : }
+    6407             : 
+    6408             : //}
+    6409             : 
+    6410             : /* isFlyingNormally() //{ */
+    6411             : 
+    6412        9601 : bool ControlManager::isFlyingNormally(void) {
+    6413             : 
+    6414        7730 :   return callbacks_enabled_ && (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6415        5236 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6416        5236 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6417       18415 :           _controller_names_.size() == 1) &&
+    6418       13753 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6419             : }
+    6420             : 
+    6421             : //}
+    6422             : 
+    6423             : /* //{ getMass() */
+    6424             : 
+    6425         297 : double ControlManager::getMass(void) {
+    6426             : 
+    6427         594 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6428             : 
+    6429         297 :   if (last_control_output.diagnostics.mass_estimator) {
+    6430          12 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6431             :   } else {
+    6432         285 :     return _uav_mass_;
+    6433             :   }
+    6434             : }
+    6435             : 
+    6436             : //}
+    6437             : 
+    6438             : /* loadConfigFile() //{ */
+    6439             : 
+    6440           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6441             : 
+    6442           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6443             : 
+    6444           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6445             : 
+    6446             :   // load the user-requested file
+    6447             :   {
+    6448           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6449           0 :     int         result  = std::system(command.c_str());
+    6450             : 
+    6451           0 :     if (result != 0) {
+    6452           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6453           0 :       return false;
+    6454             :     }
+    6455             :   }
+    6456             : 
+    6457             :   // load the platform config
+    6458           0 :   if (_platform_config_ != "") {
+    6459           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6460           0 :     int         result  = std::system(command.c_str());
+    6461             : 
+    6462           0 :     if (result != 0) {
+    6463           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6464           0 :       return false;
+    6465             :     }
+    6466             :   }
+    6467             : 
+    6468             :   // load the custom config
+    6469           0 :   if (_custom_config_ != "") {
+    6470           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6471           0 :     int         result  = std::system(command.c_str());
+    6472             : 
+    6473           0 :     if (result != 0) {
+    6474           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6475           0 :       return false;
+    6476             :     }
+    6477             :   }
+    6478             : 
+    6479           0 :   return true;
+    6480             : }
+    6481             : 
+    6482             : //}
+    6483             : 
+    6484             : // | ----------------------- safety area ---------------------- |
+    6485             : 
+    6486             : /* //{ isPointInSafetyArea3d() */
+    6487             : 
+    6488         989 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6489             : 
+    6490         989 :   if (!use_safety_area_) {
+    6491         475 :     return true;
+    6492             :   }
+    6493             : 
+    6494        1028 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6495             : 
+    6496         514 :   if (!tfed_horizontal) {
+    6497           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6498           0 :     return false;
+    6499             :   }
+    6500             : 
+    6501         514 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6502           0 :     return false;
+    6503             :   }
+    6504             : 
+    6505         514 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6506           0 :     return false;
+    6507             :   }
+    6508             : 
+    6509         514 :   return true;
+    6510             : }
+    6511             : 
+    6512             : //}
+    6513             : 
+    6514             : /* //{ isPointInSafetyArea2d() */
+    6515             : 
+    6516        2974 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6517             : 
+    6518        2974 :   if (!use_safety_area_) {
+    6519         235 :     return true;
+    6520             :   }
+    6521             : 
+    6522        5478 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6523             : 
+    6524        2739 :   if (!tfed_horizontal) {
+    6525           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6526           0 :     return false;
+    6527             :   }
+    6528             : 
+    6529        2739 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6530          76 :     return false;
+    6531             :   }
+    6532             : 
+    6533        2663 :   return true;
+    6534             : }
+    6535             : 
+    6536             : //}
+    6537             : 
+    6538             : /* //{ isPathToPointInSafetyArea3d() */
+    6539             : 
+    6540         508 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6541             : 
+    6542         508 :   if (!use_safety_area_) {
+    6543         475 :     return true;
+    6544             :   }
+    6545             : 
+    6546          33 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6547           0 :     return false;
+    6548             :   }
+    6549             : 
+    6550          66 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6551             : 
+    6552             :   {
+    6553          33 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6554             : 
+    6555          33 :     if (!ret) {
+    6556             : 
+    6557           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6558             : 
+    6559           0 :       return false;
+    6560             :     }
+    6561             : 
+    6562          33 :     start_transformed = ret.value();
+    6563             :   }
+    6564             : 
+    6565             :   {
+    6566          33 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6567             : 
+    6568          33 :     if (!ret) {
+    6569             : 
+    6570           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6571             : 
+    6572           0 :       return false;
+    6573             :     }
+    6574             : 
+    6575          33 :     end_transformed = ret.value();
+    6576             :   }
+    6577             : 
+    6578          33 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6579          33 :                                    end_transformed.reference.position.y);
+    6580             : }
+    6581             : 
+    6582             : //}
+    6583             : 
+    6584             : /* //{ isPathToPointInSafetyArea2d() */
+    6585             : 
+    6586          32 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6587          32 :   if (!use_safety_area_) {
+    6588           0 :     return true;
+    6589             :   }
+    6590             : 
+    6591          64 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6592             : 
+    6593          32 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6594           0 :     return false;
+    6595             :   }
+    6596             : 
+    6597             :   {
+    6598          32 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6599             : 
+    6600          32 :     if (!ret) {
+    6601             : 
+    6602           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6603             : 
+    6604           0 :       return false;
+    6605             :     }
+    6606             : 
+    6607          32 :     start_transformed = ret.value();
+    6608             :   }
+    6609             : 
+    6610             :   {
+    6611          32 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6612             : 
+    6613          32 :     if (!ret) {
+    6614             : 
+    6615           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6616             : 
+    6617           0 :       return false;
+    6618             :     }
+    6619             : 
+    6620          32 :     end_transformed = ret.value();
+    6621             :   }
+    6622             : 
+    6623          32 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6624          32 :                                    end_transformed.reference.position.y);
+    6625             : }
+    6626             : 
+    6627             : //}
+    6628             : 
+    6629             : /* //{ getMaxZ() */
+    6630             : 
+    6631        7567 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6632             : 
+    6633             :   // | ------- first, calculate max_z from the safety area ------ |
+    6634             : 
+    6635        7567 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6636             : 
+    6637             :   {
+    6638             : 
+    6639       15134 :     geometry_msgs::PointStamped point;
+    6640             : 
+    6641        7567 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6642        7567 :     point.point.x         = 0;
+    6643        7567 :     point.point.y         = 0;
+    6644        7567 :     point.point.z         = _safety_area_max_z_;
+    6645             : 
+    6646        7567 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6647             : 
+    6648        7567 :     if (!ret) {
+    6649           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6650             :     }
+    6651             : 
+    6652        7567 :     safety_area_max_z = ret->point.z;
+    6653             :   }
+    6654             : 
+    6655             :   // | ------------ overwrite from estimation manager ----------- |
+    6656             : 
+    6657        7567 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6658             : 
+    6659             :   {
+    6660             :     // if possible, override it with max z from the estimation manager
+    6661        7567 :     if (sh_max_z_.hasMsg()) {
+    6662             : 
+    6663       15120 :       auto msg = sh_max_z_.getMsg();
+    6664             : 
+    6665             :       // transform it into the safety area frame
+    6666       15120 :       geometry_msgs::PointStamped point;
+    6667        7560 :       point.header  = msg->header;
+    6668        7560 :       point.point.x = 0;
+    6669        7560 :       point.point.y = 0;
+    6670        7560 :       point.point.z = msg->value;
+    6671             : 
+    6672        7560 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6673             : 
+    6674        7560 :       if (!ret) {
+    6675           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6676             :       }
+    6677             : 
+    6678        7560 :       estimation_manager_max_z = ret->point.z;
+    6679             :     }
+    6680             :   }
+    6681             : 
+    6682        7567 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6683        7204 :     return estimation_manager_max_z;
+    6684             :   } else {
+    6685         363 :     return safety_area_max_z;
+    6686             :   }
+    6687             : }
+    6688             : 
+    6689             : //}
+    6690             : 
+    6691             : /* //{ getMinZ() */
+    6692             : 
+    6693       55326 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6694             : 
+    6695       55326 :   if (!use_safety_area_) {
+    6696        5338 :     return std::numeric_limits<double>::lowest();
+    6697             :   }
+    6698             : 
+    6699       99976 :   geometry_msgs::PointStamped point;
+    6700             : 
+    6701       49988 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6702       49988 :   point.point.x         = 0;
+    6703       49988 :   point.point.y         = 0;
+    6704       49988 :   point.point.z         = _safety_area_min_z_;
+    6705             : 
+    6706       99976 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6707             : 
+    6708       49988 :   if (!ret) {
+    6709           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6710           0 :     return std::numeric_limits<double>::lowest();
+    6711             :   }
+    6712             : 
+    6713       49988 :   return ret->point.z;
+    6714             : }
+    6715             : 
+    6716             : //}
+    6717             : 
+    6718             : // | --------------------- obstacle bumper -------------------- |
+    6719             : 
+    6720             : /* bumperPushFromObstacle() //{ */
+    6721             : 
+    6722           0 : bool ControlManager::bumperPushFromObstacle(void) {
+    6723           0 :   if (!bumper_enabled_) {
+    6724           0 :     return true;
+    6725             :   }
+    6726             : 
+    6727           0 :   if (!sh_bumper_.hasMsg()) {
+    6728           0 :     return true;
+    6729             :   }
+    6730             : 
+    6731             :   // copy member variables
+    6732           0 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6733           0 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6734             : 
+    6735           0 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6736             : 
+    6737           0 :   double direction                     = 0;
+    6738           0 :   double repulsion_distance            = std::numeric_limits<double>::max();
+    6739           0 :   bool   horizontal_collision_detected = false;
+    6740             : 
+    6741           0 :   bool vertical_collision_detected = false;
+    6742             : 
+    6743           0 :   for (int i = 0; i < int(bumper_data->n_horizontal_sectors); i++) {
+    6744             : 
+    6745           0 :     if (bumper_data->sectors[i] < 0) {
+    6746           0 :       continue;
+    6747             :     }
+    6748             : 
+    6749           0 :     bool wall_locked_horizontal = false;
+    6750             : 
+    6751             :     // if the sector is under critical distance
+    6752           0 :     if (bumper_data->sectors[i] <= _bumper_horizontal_distance_ && bumper_data->sectors[i] < repulsion_distance) {
+    6753             : 
+    6754             :       // check for locking between the oposite walls
+    6755             :       // get the desired direction of motion
+    6756           0 :       double oposite_direction  = double(i) * sector_size + M_PI;
+    6757           0 :       int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6758             : 
+    6759           0 :       if (bumper_data->sectors[oposite_sector_idx] > 0 &&
+    6760           0 :           ((bumper_data->sectors[i] + bumper_data->sectors[oposite_sector_idx]) <= (2 * _bumper_horizontal_distance_ + 2 * _bumper_horizontal_overshoot_))) {
+    6761             : 
+    6762           0 :         wall_locked_horizontal = true;
+    6763             : 
+    6764           0 :         if (fabs(bumper_data->sectors[i] - bumper_data->sectors[oposite_sector_idx]) <= 2 * _bumper_horizontal_overshoot_) {
+    6765             : 
+    6766           0 :           ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: locked between two walls");
+    6767           0 :           continue;
+    6768             :         }
+    6769             :       }
+    6770             : 
+    6771             :       // get the id of the oposite sector
+    6772           0 :       direction = oposite_direction;
+    6773             : 
+    6774             :       /* int oposite_sector_idx = (i + bumper_data->n_horizontal_sectors / 2) % bumper_data->n_horizontal_sectors; */
+    6775             : 
+    6776           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %d vs. %d), obstacle distance: %.2f, repulsing", i,
+    6777             :                         oposite_sector_idx, bumper_data->sectors[i]);
+    6778             : 
+    6779           0 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: oposite direction: %.2f", oposite_direction);
+    6780             : 
+    6781           0 :       if (wall_locked_horizontal) {
+    6782           0 :         if (bumper_data->sectors[i] < bumper_data->sectors[oposite_sector_idx]) {
+    6783           0 :           repulsion_distance = _bumper_horizontal_overshoot_;
+    6784             :         } else {
+    6785           0 :           repulsion_distance = -_bumper_horizontal_overshoot_;
+    6786             :         }
+    6787             :       } else {
+    6788           0 :         repulsion_distance = _bumper_horizontal_distance_ + _bumper_horizontal_overshoot_ - bumper_data->sectors[i];
+    6789             :       }
+    6790             : 
+    6791           0 :       horizontal_collision_detected = true;
+    6792             :     }
+    6793             :   }
+    6794             : 
+    6795           0 :   bool   collision_above             = false;
+    6796           0 :   bool   collision_below             = false;
+    6797           0 :   double vertical_repulsion_distance = 0;
+    6798             : 
+    6799             :   // check for vertical collision down
+    6800           0 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors] > 0 && bumper_data->sectors[bumper_data->n_horizontal_sectors] <= _bumper_vertical_distance_) {
+    6801             : 
+    6802           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    6803           0 :     collision_above             = true;
+    6804           0 :     vertical_collision_detected = true;
+    6805           0 :     vertical_repulsion_distance = _bumper_vertical_distance_ - bumper_data->sectors[bumper_data->n_horizontal_sectors];
+    6806             :   }
+    6807             : 
+    6808             :   // check for vertical collision up
+    6809           0 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] > 0 &&
+    6810           0 :       bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] <= _bumper_vertical_distance_) {
+    6811             : 
+    6812           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    6813           0 :     collision_below             = true;
+    6814           0 :     vertical_collision_detected = true;
+    6815           0 :     vertical_repulsion_distance = -(_bumper_vertical_distance_ - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]);
+    6816             :   }
+    6817             : 
+    6818             :   // check the up/down wall locking
+    6819           0 :   if (collision_above && collision_below) {
+    6820             : 
+    6821           0 :     if (((bumper_data->sectors[bumper_data->n_horizontal_sectors] + bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) <=
+    6822           0 :          (2 * _bumper_vertical_distance_ + 2 * _bumper_vertical_overshoot_))) {
+    6823             : 
+    6824           0 :       vertical_repulsion_distance =
+    6825           0 :           (-bumper_data->sectors[bumper_data->n_horizontal_sectors] + bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) / 2.0;
+    6826             : 
+    6827           0 :       if (fabs(bumper_data->sectors[bumper_data->n_horizontal_sectors] - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]) <=
+    6828           0 :           2 * _bumper_vertical_overshoot_) {
+    6829             : 
+    6830           0 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: locked between the floor and ceiling");
+    6831           0 :         vertical_collision_detected = false;
+    6832             :       }
+    6833             :     }
+    6834             :   }
+    6835             : 
+    6836             :   // if potential collision was detected and we should start the repulsing_
+    6837           0 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    6838             : 
+    6839           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: repulsion was initiated");
+    6840             : 
+    6841           0 :     if (!repulsing_) {
+    6842             : 
+    6843           0 :       if (_bumper_switch_tracker_) {
+    6844             : 
+    6845           0 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6846           0 :         std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6847             : 
+    6848             :         // remember the previously active tracker
+    6849           0 :         bumper_previous_tracker_ = active_tracker_name;
+    6850             : 
+    6851           0 :         if (active_tracker_name != _bumper_tracker_name_) {
+    6852             : 
+    6853           0 :           switchTracker(_bumper_tracker_name_);
+    6854             :         }
+    6855             :       }
+    6856             : 
+    6857           0 :       if (_bumper_switch_controller_) {
+    6858             : 
+    6859           0 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6860           0 :         std::string active_controller_name = _controller_names_[active_controller_idx];
+    6861             : 
+    6862             :         // remember the previously active controller
+    6863           0 :         bumper_previous_controller_ = active_controller_name;
+    6864             : 
+    6865           0 :         if (active_controller_name != _bumper_controller_name_) {
+    6866             : 
+    6867           0 :           switchController(_bumper_controller_name_);
+    6868             :         }
+    6869             :       }
+    6870             :     }
+    6871             : 
+    6872           0 :     repulsing_ = true;
+    6873             : 
+    6874           0 :     mrs_msgs::BumperStatus bumper_status;
+    6875           0 :     bumper_status.repulsing = repulsing_;
+    6876             : 
+    6877           0 :     ph_bumper_status_.publish(bumper_status);
+    6878             : 
+    6879           0 :     callbacks_enabled_ = false;
+    6880             : 
+    6881           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    6882             : 
+    6883           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6884             : 
+    6885             :     // create the reference in the fcu_untilted frame
+    6886           0 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    6887             : 
+    6888           0 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    6889             : 
+    6890           0 :     if (horizontal_collision_detected) {
+    6891           0 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    6892           0 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    6893             :     } else {
+    6894           0 :       reference_fcu_untilted.reference.position.x = 0;
+    6895           0 :       reference_fcu_untilted.reference.position.y = 0;
+    6896             :     }
+    6897             : 
+    6898           0 :     reference_fcu_untilted.reference.heading = 0;
+    6899             : 
+    6900           0 :     if (vertical_collision_detected) {
+    6901           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    6902             :     } else {
+    6903           0 :       reference_fcu_untilted.reference.position.z = 0;
+    6904             :     }
+    6905             : 
+    6906             :     {
+    6907           0 :       std::scoped_lock lock(mutex_tracker_list_);
+    6908             : 
+    6909             :       // transform the reference into the currently used frame
+    6910             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    6911             :       // to the tracker before we actually call the goto service
+    6912             : 
+    6913           0 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    6914             : 
+    6915           0 :       if (!ret) {
+    6916             : 
+    6917           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    6918           0 :         return false;
+    6919             :       }
+    6920             : 
+    6921           0 :       reference_fcu_untilted = ret.value();
+    6922             : 
+    6923             :       // copy the reference into the service type message
+    6924           0 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    6925           0 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    6926             : 
+    6927             :       // disable callbacks of all trackers
+    6928           0 :       req_enable_callbacks.data = false;
+    6929           0 :       for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6930           0 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6931             :       }
+    6932             : 
+    6933             :       // enable the callbacks for the active tracker
+    6934           0 :       req_enable_callbacks.data = true;
+    6935           0 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6936             : 
+    6937             :       // call the goto
+    6938           0 :       tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    6939           0 :           mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    6940             : 
+    6941             :       // disable the callbacks back again
+    6942           0 :       req_enable_callbacks.data = false;
+    6943           0 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6944             :     }
+    6945             :   }
+    6946             : 
+    6947             :   // if repulsing_ and the distance is safe once again
+    6948           0 :   if ((repulsing_ && !horizontal_collision_detected && !vertical_collision_detected)) {
+    6949             : 
+    6950           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: repulsion was stopped");
+    6951             : 
+    6952           0 :     if (_bumper_switch_tracker_) {
+    6953             : 
+    6954           0 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6955           0 :       std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6956             : 
+    6957           0 :       if (active_tracker_name != bumper_previous_tracker_) {
+    6958             : 
+    6959           0 :         switchTracker(bumper_previous_tracker_);
+    6960             :       }
+    6961             :     }
+    6962             : 
+    6963           0 :     if (_bumper_switch_controller_) {
+    6964             : 
+    6965           0 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6966           0 :       std::string active_controller_name = _controller_names_[active_controller_idx];
+    6967             : 
+    6968           0 :       if (active_controller_name != bumper_previous_controller_) {
+    6969             : 
+    6970           0 :         switchController(bumper_previous_controller_);
+    6971             :       }
+    6972             :     }
+    6973             : 
+    6974           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6975             : 
+    6976             :     {
+    6977           0 :       std::scoped_lock lock(mutex_tracker_list_);
+    6978             : 
+    6979             :       // enable callbacks of all trackers
+    6980           0 :       req_enable_callbacks.data = true;
+    6981           0 :       for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6982           0 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6983             :       }
+    6984             :     }
+    6985             : 
+    6986           0 :     callbacks_enabled_ = true;
+    6987             : 
+    6988           0 :     repulsing_ = false;
+    6989             :   }
+    6990             : 
+    6991           0 :   return false;
+    6992             : }
+    6993             : 
+    6994             : //}
+    6995             : 
+    6996             : /* bumperGetSectorId() //{ */
+    6997             : 
+    6998           0 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    6999             :   // copy member variables
+    7000           0 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    7001             : 
+    7002             :   // heading of the point in drone frame
+    7003           0 :   double point_heading_horizontal = atan2(y, x);
+    7004             : 
+    7005           0 :   point_heading_horizontal += TAU;
+    7006             : 
+    7007             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7008           0 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7009           0 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7010             :   }
+    7011             : 
+    7012             :   // heading of the right edge of the first sector
+    7013           0 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7014             : 
+    7015             :   // calculate the idx
+    7016           0 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7017             : 
+    7018           0 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7019           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7020             :   }
+    7021             : 
+    7022           0 :   return idx;
+    7023             : }
+    7024             : 
+    7025             : //}
+    7026             : 
+    7027             : // | ------------------------- safety ------------------------- |
+    7028             : 
+    7029             : /* //{ changeLandingState() */
+    7030             : 
+    7031           8 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7032             :   // copy member variables
+    7033          16 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7034             : 
+    7035             :   {
+    7036           8 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7037             : 
+    7038           8 :     previous_state_landing_ = current_state_landing_;
+    7039           8 :     current_state_landing_  = new_state;
+    7040             :   }
+    7041             : 
+    7042           8 :   switch (current_state_landing_) {
+    7043             : 
+    7044           3 :     case IDLE_STATE:
+    7045           3 :       break;
+    7046           5 :     case LANDING_STATE: {
+    7047             : 
+    7048           5 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7049           5 :       timer_eland_.start();
+    7050           5 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7051           5 :       eland_triggered_ = true;
+    7052           5 :       bumper_enabled_  = false;
+    7053             : 
+    7054           5 :       landing_uav_mass_ = getMass();
+    7055             :     }
+    7056             : 
+    7057           5 :     break;
+    7058             :   }
+    7059             : 
+    7060           8 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7061           8 : }
+    7062             : 
+    7063             : //}
+    7064             : 
+    7065             : /* hover() //{ */
+    7066             : 
+    7067           0 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7068           0 :   if (!is_initialized_)
+    7069           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7070             : 
+    7071           0 :   if (eland_triggered_)
+    7072           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7073             : 
+    7074           0 :   if (failsafe_triggered_)
+    7075           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7076             : 
+    7077             :   {
+    7078           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7079             : 
+    7080           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7081           0 :     std_srvs::TriggerRequest            request;
+    7082             : 
+    7083           0 :     response = tracker_list_[active_tracker_idx_]->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7084             : 
+    7085           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7086             : 
+    7087           0 :       return std::tuple(response->success, response->message);
+    7088             : 
+    7089             :     } else {
+    7090             : 
+    7091           0 :       std::stringstream ss;
+    7092           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'hover()' function!";
+    7093             : 
+    7094           0 :       return std::tuple(false, ss.str());
+    7095             :     }
+    7096             :   }
+    7097             : }
+    7098             : 
+    7099             : //}
+    7100             : 
+    7101             : /* //{ ehover() */
+    7102             : 
+    7103           2 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7104           2 :   if (!is_initialized_)
+    7105           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7106             : 
+    7107           2 :   if (eland_triggered_)
+    7108           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7109             : 
+    7110           2 :   if (failsafe_triggered_)
+    7111           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7112             : 
+    7113             :   // copy the member variables
+    7114           4 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7115           4 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7116           2 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7117             : 
+    7118           2 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7119             : 
+    7120           0 :     std::stringstream ss;
+    7121           0 :     ss << "can not trigger ehover while not flying";
+    7122           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7123             : 
+    7124           0 :     return std::tuple(false, ss.str());
+    7125             :   }
+    7126             : 
+    7127           2 :   ungripSrv();
+    7128             : 
+    7129             :   {
+    7130             : 
+    7131           2 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7132             : 
+    7133             :     // check if the tracker was successfully switched
+    7134             :     // this is vital, that is the core of the hover
+    7135           2 :     if (!success) {
+    7136             : 
+    7137           0 :       std::stringstream ss;
+    7138           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7139           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7140             : 
+    7141           0 :       return std::tuple(success, ss.str());
+    7142             :     }
+    7143             :   }
+    7144             : 
+    7145             :   {
+    7146           4 :     auto [success, message] = switchController(_eland_controller_name_);
+    7147             : 
+    7148             :     // check if the controller was successfully switched
+    7149             :     // this is not vital, we can continue without that
+    7150           2 :     if (!success) {
+    7151             : 
+    7152           0 :       std::stringstream ss;
+    7153           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7154           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7155             :     }
+    7156             :   }
+    7157             : 
+    7158           2 :   std::stringstream ss;
+    7159           2 :   ss << "ehover activated";
+    7160           2 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7161             : 
+    7162           2 :   callbacks_enabled_ = false;
+    7163             : 
+    7164           2 :   return std::tuple(true, ss.str());
+    7165             : }
+    7166             : 
+    7167             : //}
+    7168             : 
+    7169             : /* eland() //{ */
+    7170             : 
+    7171           5 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7172           5 :   if (!is_initialized_)
+    7173           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7174             : 
+    7175           5 :   if (eland_triggered_)
+    7176           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7177             : 
+    7178           5 :   if (failsafe_triggered_)
+    7179           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7180             : 
+    7181             :   // copy member variables
+    7182          10 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7183          10 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7184           5 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7185             : 
+    7186           5 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7187             : 
+    7188           0 :     std::stringstream ss;
+    7189           0 :     ss << "can not trigger eland while not flying";
+    7190           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7191             : 
+    7192           0 :     return std::tuple(false, ss.str());
+    7193             :   }
+    7194             : 
+    7195           5 :   if (_rc_emergency_handoff_) {
+    7196             : 
+    7197           0 :     toggleOutput(false);
+    7198             : 
+    7199           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7200             :   }
+    7201             : 
+    7202             :   {
+    7203           5 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7204             : 
+    7205             :     // check if the tracker was successfully switched
+    7206             :     // this is vital
+    7207           5 :     if (!success) {
+    7208             : 
+    7209           0 :       std::stringstream ss;
+    7210           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7211           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7212             : 
+    7213           0 :       return std::tuple(success, ss.str());
+    7214             :     }
+    7215             :   }
+    7216             : 
+    7217             :   {
+    7218          10 :     auto [success, message] = switchController(_eland_controller_name_);
+    7219             : 
+    7220             :     // check if the controller was successfully switched
+    7221             :     // this is not vital, we can continue without it
+    7222           5 :     if (!success) {
+    7223             : 
+    7224           0 :       std::stringstream ss;
+    7225           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7226           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7227             :     }
+    7228             :   }
+    7229             : 
+    7230             :   // | ----------------- call the eland service ----------------- |
+    7231             : 
+    7232           5 :   std::stringstream ss;
+    7233             :   bool              success;
+    7234             : 
+    7235           5 :   if (elandSrv()) {
+    7236             : 
+    7237           5 :     changeLandingState(LANDING_STATE);
+    7238             : 
+    7239           5 :     odometryCallbacksSrv(false);
+    7240             : 
+    7241           5 :     ss << "eland activated";
+    7242           5 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7243             : 
+    7244           5 :     success = true;
+    7245             : 
+    7246           5 :     callbacks_enabled_ = false;
+    7247             : 
+    7248             :   } else {
+    7249             : 
+    7250           0 :     ss << "error during activation of eland";
+    7251           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7252             : 
+    7253           0 :     success = false;
+    7254             :   }
+    7255             : 
+    7256          10 :   return std::tuple(success, ss.str());
+    7257             : }
+    7258             : 
+    7259             : //}
+    7260             : 
+    7261             : /* failsafe() //{ */
+    7262             : 
+    7263           7 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7264             : 
+    7265             :   // copy member variables
+    7266          14 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7267          14 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7268           7 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7269           7 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7270             : 
+    7271           7 :   if (!is_initialized_) {
+    7272           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7273             :   }
+    7274             : 
+    7275           7 :   if (failsafe_triggered_) {
+    7276           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7277             :   }
+    7278             : 
+    7279           7 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7280             : 
+    7281           0 :     std::stringstream ss;
+    7282           0 :     ss << "can not trigger failsafe while not flying";
+    7283           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7284           0 :     return std::tuple(false, ss.str());
+    7285             :   }
+    7286             : 
+    7287           7 :   if (_rc_emergency_handoff_) {
+    7288             : 
+    7289           0 :     toggleOutput(false);
+    7290             : 
+    7291           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7292             :   }
+    7293             : 
+    7294           7 :   if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+    7295           0 :     return eland();
+    7296             :   }
+    7297             : 
+    7298           7 :   if (_parachute_enabled_) {
+    7299             : 
+    7300           0 :     auto [success, message] = deployParachute();
+    7301             : 
+    7302           0 :     if (success) {
+    7303             : 
+    7304           0 :       std::stringstream ss;
+    7305           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7306           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7307             : 
+    7308           0 :       return std::tuple(true, ss.str());
+    7309             : 
+    7310             :     } else {
+    7311             : 
+    7312           0 :       std::stringstream ss;
+    7313           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7314           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7315             :     }
+    7316             :   }
+    7317             : 
+    7318           7 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7319             : 
+    7320             :     try {
+    7321             : 
+    7322          14 :       std::scoped_lock lock(mutex_controller_list_);
+    7323             : 
+    7324           7 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7325           7 :       controller_list_[_failsafe_controller_idx_]->activate(last_control_output);
+    7326             : 
+    7327             :       {
+    7328          14 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7329             : 
+    7330             :         // update the time (used in failsafe)
+    7331           7 :         controller_tracker_switch_time_ = ros::Time::now();
+    7332             :       }
+    7333             : 
+    7334           7 :       failsafe_triggered_ = true;
+    7335           7 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7336           7 :       timer_eland_.stop();
+    7337           7 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7338             : 
+    7339           7 :       landing_uav_mass_ = getMass();
+    7340             : 
+    7341           7 :       eland_triggered_ = false;
+    7342           7 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7343           7 :       timer_failsafe_.start();
+    7344           7 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7345             : 
+    7346           7 :       bumper_enabled_ = false;
+    7347             : 
+    7348           7 :       odometryCallbacksSrv(false);
+    7349             : 
+    7350           7 :       callbacks_enabled_ = false;
+    7351             : 
+    7352           7 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7353             : 
+    7354             :       // super important, switch the active controller idx
+    7355             :       try {
+    7356           7 :         controller_list_[active_controller_idx_]->deactivate();
+    7357           7 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7358             :       }
+    7359           0 :       catch (std::runtime_error& exrun) {
+    7360           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    7361             :       }
+    7362             :     }
+    7363           0 :     catch (std::runtime_error& exrun) {
+    7364           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7365           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7366             :     }
+    7367             :   }
+    7368             : 
+    7369          14 :   return std::tuple(true, "failsafe activated");
+    7370             : }
+    7371             : 
+    7372             : //}
+    7373             : 
+    7374             : /* escalatingFailsafe() //{ */
+    7375             : 
+    7376         146 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7377         292 :   std::stringstream ss;
+    7378             : 
+    7379         146 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7380             : 
+    7381         138 :     ss << "too soon for escalating failsafe";
+    7382         138 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7383             : 
+    7384         138 :     return std::tuple(false, ss.str());
+    7385             :   }
+    7386             : 
+    7387           8 :   if (!output_enabled_) {
+    7388             : 
+    7389           0 :     ss << "not escalating failsafe, output is disabled";
+    7390           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7391             : 
+    7392           0 :     return std::tuple(false, ss.str());
+    7393             :   }
+    7394             : 
+    7395           8 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7396             : 
+    7397           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7398           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7399             : 
+    7400          16 :   std::string active_tracker_name    = _tracker_names_[active_tracker_idx];
+    7401          16 :   std::string active_controller_name = _controller_names_[active_controller_idx];
+    7402             : 
+    7403           8 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7404             : 
+    7405           8 :   escalating_failsafe_time_ = ros::Time::now();
+    7406             : 
+    7407           8 :   switch (next_state) {
+    7408             : 
+    7409           0 :     case ESC_NONE_STATE: {
+    7410             : 
+    7411           0 :       ss << "escalating failsafe has run to impossible situation";
+    7412           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7413             : 
+    7414           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7415             : 
+    7416             :       break;
+    7417             :     }
+    7418             : 
+    7419           2 :     case ESC_EHOVER_STATE: {
+    7420             : 
+    7421           2 :       ss << "escalating failsafe escalates to ehover";
+    7422           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7423             : 
+    7424           4 :       auto [success, message] = ehover();
+    7425             : 
+    7426           2 :       if (success) {
+    7427           2 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7428             :       }
+    7429             : 
+    7430           2 :       return {success, message};
+    7431             : 
+    7432             :       break;
+    7433             :     }
+    7434             : 
+    7435           2 :     case ESC_ELAND_STATE: {
+    7436             : 
+    7437           2 :       ss << "escalating failsafe escalates to eland";
+    7438           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7439             : 
+    7440           4 :       auto [success, message] = eland();
+    7441             : 
+    7442           2 :       if (success) {
+    7443           2 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7444             :       }
+    7445             : 
+    7446           2 :       return {success, message};
+    7447             : 
+    7448             :       break;
+    7449             :     }
+    7450             : 
+    7451           2 :     case ESC_FAILSAFE_STATE: {
+    7452             : 
+    7453           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7454             : 
+    7455           2 :       ss << "escalating failsafe escalates to failsafe";
+    7456           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7457             : 
+    7458           4 :       auto [success, message] = failsafe();
+    7459             : 
+    7460           2 :       if (success) {
+    7461           2 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7462             :       }
+    7463             : 
+    7464           2 :       return {success, message};
+    7465             : 
+    7466             :       break;
+    7467             :     }
+    7468             : 
+    7469           2 :     case ESC_FINISHED_STATE: {
+    7470             : 
+    7471           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7472             : 
+    7473           2 :       ss << "escalating failsafe has nothing more to do";
+    7474           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7475             : 
+    7476           4 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7477             : 
+    7478             :       break;
+    7479             :     }
+    7480             : 
+    7481           0 :     default: {
+    7482             : 
+    7483           0 :       break;
+    7484             :     }
+    7485             :   }
+    7486             : 
+    7487           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7488             : 
+    7489           0 :   return std::tuple(false, "escalating failsafe exception");
+    7490             : }
+    7491             : 
+    7492             : //}
+    7493             : 
+    7494             : /* getNextEscFailsafeState() //{ */
+    7495             : 
+    7496           8 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7497           8 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7498             : 
+    7499           8 :   switch (current_state) {
+    7500             : 
+    7501           2 :     case ESC_FINISHED_STATE: {
+    7502             : 
+    7503           2 :       return ESC_FINISHED_STATE;
+    7504             : 
+    7505             :       break;
+    7506             :     }
+    7507             : 
+    7508           2 :     case ESC_NONE_STATE: {
+    7509             : 
+    7510           2 :       if (_escalating_failsafe_ehover_) {
+    7511           2 :         return ESC_EHOVER_STATE;
+    7512           0 :       } else if (_escalating_failsafe_eland_) {
+    7513           0 :         return ESC_ELAND_STATE;
+    7514           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7515           0 :         return ESC_FAILSAFE_STATE;
+    7516             :       } else {
+    7517           0 :         return ESC_FINISHED_STATE;
+    7518             :       }
+    7519             : 
+    7520             :       break;
+    7521             :     }
+    7522             : 
+    7523           2 :     case ESC_EHOVER_STATE: {
+    7524             : 
+    7525           2 :       if (_escalating_failsafe_eland_) {
+    7526           2 :         return ESC_ELAND_STATE;
+    7527           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7528           0 :         return ESC_FAILSAFE_STATE;
+    7529             :       } else {
+    7530           0 :         return ESC_FINISHED_STATE;
+    7531             :       }
+    7532             : 
+    7533             :       break;
+    7534             :     }
+    7535             : 
+    7536           2 :     case ESC_ELAND_STATE: {
+    7537             : 
+    7538           2 :       if (_escalating_failsafe_failsafe_) {
+    7539           2 :         return ESC_FAILSAFE_STATE;
+    7540             :       } else {
+    7541           0 :         return ESC_FINISHED_STATE;
+    7542             :       }
+    7543             : 
+    7544             :       break;
+    7545             :     }
+    7546             : 
+    7547           0 :     case ESC_FAILSAFE_STATE: {
+    7548             : 
+    7549           0 :       return ESC_FINISHED_STATE;
+    7550             : 
+    7551             :       break;
+    7552             :     }
+    7553             :   }
+    7554             : 
+    7555           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7556             : 
+    7557           0 :   return ESC_NONE_STATE;
+    7558             : }
+    7559             : 
+    7560             : //}
+    7561             : 
+    7562             : // | ------------------- trajectory tracking ------------------ |
+    7563             : 
+    7564             : /* startTrajectoryTracking() //{ */
+    7565             : 
+    7566           0 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7567           0 :   if (!is_initialized_)
+    7568           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7569             : 
+    7570             :   {
+    7571           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7572             : 
+    7573           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7574           0 :     std_srvs::TriggerRequest            request;
+    7575             : 
+    7576             :     response =
+    7577           0 :         tracker_list_[active_tracker_idx_]->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7578             : 
+    7579           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7580             : 
+    7581           0 :       return std::tuple(response->success, response->message);
+    7582             : 
+    7583             :     } else {
+    7584             : 
+    7585           0 :       std::stringstream ss;
+    7586           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'startTrajectoryTracking()' function!";
+    7587             : 
+    7588           0 :       return std::tuple(false, ss.str());
+    7589             :     }
+    7590             :   }
+    7591             : }
+    7592             : 
+    7593             : //}
+    7594             : 
+    7595             : /* stopTrajectoryTracking() //{ */
+    7596             : 
+    7597           0 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7598           0 :   if (!is_initialized_)
+    7599           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7600             : 
+    7601             :   {
+    7602           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7603             : 
+    7604           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7605           0 :     std_srvs::TriggerRequest            request;
+    7606             : 
+    7607             :     response =
+    7608           0 :         tracker_list_[active_tracker_idx_]->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7609             : 
+    7610           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7611             : 
+    7612           0 :       return std::tuple(response->success, response->message);
+    7613             : 
+    7614             :     } else {
+    7615             : 
+    7616           0 :       std::stringstream ss;
+    7617           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7618             : 
+    7619           0 :       return std::tuple(false, ss.str());
+    7620             :     }
+    7621             :   }
+    7622             : }
+    7623             : 
+    7624             : //}
+    7625             : 
+    7626             : /* resumeTrajectoryTracking() //{ */
+    7627             : 
+    7628           0 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7629           0 :   if (!is_initialized_)
+    7630           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7631             : 
+    7632             :   {
+    7633           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7634             : 
+    7635           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7636           0 :     std_srvs::TriggerRequest            request;
+    7637             : 
+    7638             :     response =
+    7639           0 :         tracker_list_[active_tracker_idx_]->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7640             : 
+    7641           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7642             : 
+    7643           0 :       return std::tuple(response->success, response->message);
+    7644             : 
+    7645             :     } else {
+    7646             : 
+    7647           0 :       std::stringstream ss;
+    7648           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7649             : 
+    7650           0 :       return std::tuple(false, ss.str());
+    7651             :     }
+    7652             :   }
+    7653             : }
+    7654             : 
+    7655             : //}
+    7656             : 
+    7657             : /* gotoTrajectoryStart() //{ */
+    7658             : 
+    7659           0 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7660           0 :   if (!is_initialized_)
+    7661           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7662             : 
+    7663             :   {
+    7664           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7665             : 
+    7666           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7667           0 :     std_srvs::TriggerRequest            request;
+    7668             : 
+    7669           0 :     response = tracker_list_[active_tracker_idx_]->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7670             : 
+    7671           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7672             : 
+    7673           0 :       return std::tuple(response->success, response->message);
+    7674             : 
+    7675             :     } else {
+    7676             : 
+    7677           0 :       std::stringstream ss;
+    7678           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7679             : 
+    7680           0 :       return std::tuple(false, ss.str());
+    7681             :     }
+    7682             :   }
+    7683             : }
+    7684             : 
+    7685             : //}
+    7686             : 
+    7687             : // | ----------------- service client wrappers ---------------- |
+    7688             : 
+    7689             : /* arming() //{ */
+    7690             : 
+    7691          13 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7692          26 :   std::stringstream ss;
+    7693             : 
+    7694          13 :   if (input) {
+    7695             : 
+    7696           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7697           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7698           0 :     return std::tuple(false, ss.str());
+    7699             :   }
+    7700             : 
+    7701          13 :   if (!input && !isOffboard()) {
+    7702             : 
+    7703           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7704           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7705           0 :     return std::tuple(false, ss.str());
+    7706             :   }
+    7707             : 
+    7708          13 :   if (!input && _rc_emergency_handoff_) {
+    7709             : 
+    7710           0 :     toggleOutput(false);
+    7711             : 
+    7712           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7713             :   }
+    7714             : 
+    7715          13 :   std_srvs::SetBool srv_out;
+    7716             : 
+    7717          13 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7718             : 
+    7719          13 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7720             : 
+    7721          13 :   if (sch_arming_.call(srv_out)) {
+    7722             : 
+    7723          13 :     if (srv_out.response.success) {
+    7724             : 
+    7725          13 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7726          13 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7727             : 
+    7728          13 :       if (!input) {
+    7729             : 
+    7730          13 :         toggleOutput(false);
+    7731             : 
+    7732          13 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7733          13 :         timer_failsafe_.stop();
+    7734          13 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7735             : 
+    7736          13 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7737          13 :         timer_eland_.stop();
+    7738          13 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7739             :       }
+    7740             : 
+    7741             :     } else {
+    7742           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7743           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7744             :     }
+    7745             : 
+    7746             :   } else {
+    7747           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7748           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7749             :   }
+    7750             : 
+    7751          26 :   return std::tuple(srv_out.response.success, ss.str());
+    7752             : }
+    7753             : 
+    7754             : //}
+    7755             : 
+    7756             : /* odometryCallbacksSrv() //{ */
+    7757             : 
+    7758          12 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7759          12 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7760             : 
+    7761          24 :   std_srvs::SetBool srv;
+    7762             : 
+    7763          12 :   srv.request.data = input;
+    7764             : 
+    7765          12 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7766             : 
+    7767          12 :   if (res) {
+    7768             : 
+    7769          12 :     if (!srv.response.success) {
+    7770           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7771             :     }
+    7772             : 
+    7773             :   } else {
+    7774           0 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7775             :   }
+    7776          12 : }
+    7777             : 
+    7778             : //}
+    7779             : 
+    7780             : /* elandSrv() //{ */
+    7781             : 
+    7782           5 : bool ControlManager::elandSrv(void) {
+    7783           5 :   ROS_INFO("[ControlManager]: calling for eland");
+    7784             : 
+    7785          10 :   std_srvs::Trigger srv;
+    7786             : 
+    7787           5 :   bool res = sch_eland_.call(srv);
+    7788             : 
+    7789           5 :   if (res) {
+    7790             : 
+    7791           5 :     if (!srv.response.success) {
+    7792           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    7793             :     }
+    7794             : 
+    7795           5 :     return srv.response.success;
+    7796             : 
+    7797             :   } else {
+    7798             : 
+    7799           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    7800             : 
+    7801           0 :     return false;
+    7802             :   }
+    7803             : }
+    7804             : 
+    7805             : //}
+    7806             : 
+    7807             : /* parachuteSrv() //{ */
+    7808             : 
+    7809           0 : bool ControlManager::parachuteSrv(void) {
+    7810           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    7811             : 
+    7812           0 :   std_srvs::Trigger srv;
+    7813             : 
+    7814           0 :   bool res = sch_parachute_.call(srv);
+    7815             : 
+    7816           0 :   if (res) {
+    7817             : 
+    7818           0 :     if (!srv.response.success) {
+    7819           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    7820             :     }
+    7821             : 
+    7822           0 :     return srv.response.success;
+    7823             : 
+    7824             :   } else {
+    7825             : 
+    7826           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    7827             : 
+    7828           0 :     return false;
+    7829             :   }
+    7830             : }
+    7831             : 
+    7832             : //}
+    7833             : 
+    7834             : /* ungripSrv() //{ */
+    7835             : 
+    7836         376 : void ControlManager::ungripSrv(void) {
+    7837         752 :   std_srvs::Trigger srv;
+    7838             : 
+    7839         376 :   bool res = sch_ungrip_.call(srv);
+    7840             : 
+    7841         376 :   if (res) {
+    7842             : 
+    7843           0 :     if (!srv.response.success) {
+    7844           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    7845             :     }
+    7846             : 
+    7847             :   } else {
+    7848         376 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    7849             :   }
+    7850         376 : }
+    7851             : 
+    7852             : //}
+    7853             : 
+    7854             : // | ------------------------ routines ------------------------ |
+    7855             : 
+    7856             : /* toggleOutput() //{ */
+    7857             : 
+    7858          70 : void ControlManager::toggleOutput(const bool& input) {
+    7859             : 
+    7860          70 :   if (input == output_enabled_) {
+    7861           3 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    7862           3 :     return;
+    7863             :   }
+    7864             : 
+    7865          67 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    7866             : 
+    7867          67 :   output_enabled_ = input;
+    7868             : 
+    7869             :   // if switching output off, switch to NullTracker
+    7870          67 :   if (!output_enabled_) {
+    7871             : 
+    7872          13 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    7873             : 
+    7874          13 :     switchTracker(_null_tracker_name_);
+    7875             : 
+    7876          13 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    7877             : 
+    7878          13 :     switchController(_eland_controller_name_);
+    7879             : 
+    7880             :     // | --------- deactivate all trackers and controllers -------- |
+    7881             : 
+    7882          91 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    7883             : 
+    7884          78 :       std::map<std::string, TrackerParams>::iterator it;
+    7885          78 :       it = trackers_.find(_tracker_names_[i]);
+    7886             : 
+    7887             :       try {
+    7888          78 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    7889          78 :         tracker_list_[i]->deactivate();
+    7890             :       }
+    7891           0 :       catch (std::runtime_error& ex) {
+    7892           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    7893             :       }
+    7894             :     }
+    7895             : 
+    7896          78 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    7897             : 
+    7898          65 :       std::map<std::string, ControllerParams>::iterator it;
+    7899          65 :       it = controllers_.find(_controller_names_[i]);
+    7900             : 
+    7901             :       try {
+    7902          65 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    7903          65 :         controller_list_[i]->deactivate();
+    7904             :       }
+    7905           0 :       catch (std::runtime_error& ex) {
+    7906           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    7907             :       }
+    7908             :     }
+    7909             : 
+    7910          13 :     timer_failsafe_.stop();
+    7911          13 :     timer_eland_.stop();
+    7912          13 :     timer_pirouette_.stop();
+    7913             : 
+    7914          13 :     offboard_mode_was_true_ = false;
+    7915             :   }
+    7916             : }
+    7917             : 
+    7918             : //}
+    7919             : 
+    7920             : /* switchTracker() //{ */
+    7921             : 
+    7922         134 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    7923         402 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    7924         402 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    7925             : 
+    7926             :   // copy member variables
+    7927         268 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7928         134 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7929             : 
+    7930         268 :   std::stringstream ss;
+    7931             : 
+    7932         134 :   if (!got_uav_state_) {
+    7933             : 
+    7934           0 :     ss << "can not switch tracker, missing odometry!";
+    7935           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7936           0 :     return std::tuple(false, ss.str());
+    7937             :   }
+    7938             : 
+    7939         134 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    7940             : 
+    7941           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    7942           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7943           0 :     return std::tuple(false, ss.str());
+    7944             :   }
+    7945             : 
+    7946         134 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    7947             : 
+    7948             :   // check if the tracker exists
+    7949         134 :   if (!new_tracker_idx) {
+    7950           0 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    7951           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7952           0 :     return std::tuple(false, ss.str());
+    7953             :   }
+    7954             : 
+    7955             :   // check if the tracker is already active
+    7956         134 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    7957           5 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    7958           5 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7959           5 :     return std::tuple(true, ss.str());
+    7960             :   }
+    7961             : 
+    7962             :   {
+    7963         129 :     std::scoped_lock lock(mutex_tracker_list_);
+    7964             : 
+    7965             :     try {
+    7966             : 
+    7967         129 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_[new_tracker_idx.value()].c_str());
+    7968             : 
+    7969         129 :       auto [success, message] = tracker_list_[new_tracker_idx.value()]->activate(last_tracker_cmd);
+    7970             : 
+    7971         129 :       if (!success) {
+    7972             : 
+    7973           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    7974           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7975           0 :         return std::tuple(false, ss.str());
+    7976             : 
+    7977             :       } else {
+    7978             : 
+    7979         129 :         ss << "the tracker '" << tracker_name << "' was activated";
+    7980         129 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7981             : 
+    7982             :         {
+    7983         258 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7984             : 
+    7985             :           // update the time (used in failsafe)
+    7986         129 :           controller_tracker_switch_time_ = ros::Time::now();
+    7987             :         }
+    7988             : 
+    7989             :         // super important, switch the active tracker idx
+    7990             :         try {
+    7991             : 
+    7992         129 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    7993         129 :           tracker_list_[active_tracker_idx_]->deactivate();
+    7994             : 
+    7995             :           // if switching from null tracker, re-activate the already active the controller
+    7996         129 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    7997             : 
+    7998          54 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    7999             :             {
+    8000         108 :               std::scoped_lock lock(mutex_controller_list_);
+    8001             : 
+    8002          54 :               initializeControlOutput();
+    8003             : 
+    8004         108 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8005             : 
+    8006          54 :               controller_list_[active_controller_idx_]->activate(last_control_output);
+    8007             : 
+    8008             :               {
+    8009         108 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8010             : 
+    8011             :                 // update the time (used in failsafe)
+    8012          54 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8013             :               }
+    8014             :             }
+    8015             : 
+    8016             :             // if switching to null tracker, deactivate the active controller
+    8017          75 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8018             : 
+    8019          13 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8020             :             {
+    8021          26 :               std::scoped_lock lock(mutex_controller_list_);
+    8022             : 
+    8023          13 :               controller_list_[active_controller_idx_]->deactivate();
+    8024             :             }
+    8025             : 
+    8026             :             {
+    8027          13 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8028             : 
+    8029          13 :               last_tracker_cmd_ = {};
+    8030             :             }
+    8031             : 
+    8032          13 :             initializeControlOutput();
+    8033             :           }
+    8034             : 
+    8035         129 :           active_tracker_idx_ = new_tracker_idx.value();
+    8036             :         }
+    8037           0 :         catch (std::runtime_error& exrun) {
+    8038           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8039             :         }
+    8040             :       }
+    8041             :     }
+    8042           0 :     catch (std::runtime_error& exrun) {
+    8043           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8044           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8045             :     }
+    8046             :   }
+    8047             : 
+    8048         129 :   return std::tuple(true, ss.str());
+    8049             : }
+    8050             : 
+    8051             : //}
+    8052             : 
+    8053             : /* switchController() //{ */
+    8054             : 
+    8055         131 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8056         393 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8057         393 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8058             : 
+    8059             :   // copy member variables
+    8060         262 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8061         262 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8062         131 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8063             : 
+    8064         262 :   std::stringstream ss;
+    8065             : 
+    8066         131 :   if (!got_uav_state_) {
+    8067             : 
+    8068           0 :     ss << "can not switch controller, missing odometry!";
+    8069           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8070           0 :     return std::tuple(false, ss.str());
+    8071             :   }
+    8072             : 
+    8073         131 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8074             : 
+    8075           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8076           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8077           0 :     return std::tuple(false, ss.str());
+    8078             :   }
+    8079             : 
+    8080         131 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8081             : 
+    8082             :   // check if the controller exists
+    8083         131 :   if (!new_controller_idx) {
+    8084           0 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8085           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8086           0 :     return std::tuple(false, ss.str());
+    8087             :   }
+    8088             : 
+    8089             :   // check if the controller is not active
+    8090         131 :   if (new_controller_idx.value() == active_controller_idx) {
+    8091             : 
+    8092          18 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8093          18 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8094          18 :     return std::tuple(true, ss.str());
+    8095             :   }
+    8096             : 
+    8097             :   {
+    8098         113 :     std::scoped_lock lock(mutex_controller_list_);
+    8099             : 
+    8100             :     try {
+    8101             : 
+    8102         113 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_[new_controller_idx.value()].c_str());
+    8103         113 :       if (!controller_list_[new_controller_idx.value()]->activate(last_control_output)) {
+    8104             : 
+    8105           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8106           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8107           0 :         return std::tuple(false, ss.str());
+    8108             : 
+    8109             :       } else {
+    8110             : 
+    8111         113 :         ss << "the controller '" << controller_name << "' was activated";
+    8112         113 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8113             : 
+    8114         113 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_[new_controller_idx.value()].c_str(),
+    8115             :                  _tracker_names_[active_tracker_idx_].c_str());
+    8116             : 
+    8117             :         // reactivate the current tracker
+    8118             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8119             :         // but it serves the purpose
+    8120             :         {
+    8121         113 :           std::scoped_lock lock(mutex_tracker_list_);
+    8122             : 
+    8123         113 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8124         113 :           tracker_list_[active_tracker_idx_]->activate({});
+    8125             :         }
+    8126             : 
+    8127             :         {
+    8128         226 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8129             : 
+    8130             :           // update the time (used in failsafe)
+    8131         113 :           controller_tracker_switch_time_ = ros::Time::now();
+    8132             :         }
+    8133             : 
+    8134             :         // super important, switch the active controller idx
+    8135             :         try {
+    8136             : 
+    8137         113 :           controller_list_[active_controller_idx_]->deactivate();
+    8138         113 :           active_controller_idx_ = new_controller_idx.value();
+    8139             :         }
+    8140           0 :         catch (std::runtime_error& exrun) {
+    8141           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    8142             :         }
+    8143             :       }
+    8144             :     }
+    8145           0 :     catch (std::runtime_error& exrun) {
+    8146           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8147           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8148             :     }
+    8149             :   }
+    8150             : 
+    8151         113 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8152             : 
+    8153             :   {
+    8154         113 :     std::scoped_lock lock(mutex_constraints_);
+    8155             : 
+    8156         113 :     sanitized_constraints_ = current_constraints_;
+    8157         113 :     sanitized_constraints  = sanitized_constraints_;
+    8158             :   }
+    8159             : 
+    8160         113 :   setConstraintsToControllers(sanitized_constraints);
+    8161             : 
+    8162         113 :   return std::tuple(true, ss.str());
+    8163             : }
+    8164             : 
+    8165             : //}
+    8166             : 
+    8167             : /* updateTrackers() //{ */
+    8168             : 
+    8169       64638 : void ControlManager::updateTrackers(void) {
+    8170      129276 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8171      129276 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8172             : 
+    8173             :   // copy member variables
+    8174       64638 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8175       64638 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8176       64638 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8177             : 
+    8178             :   // --------------------------------------------------------------
+    8179             :   // |                     Update the trackers                    |
+    8180             :   // --------------------------------------------------------------
+    8181             : 
+    8182           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8183             : 
+    8184             :   // for each tracker
+    8185      454146 :   for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8186             : 
+    8187      389508 :     if (i == active_tracker_idx) {
+    8188             : 
+    8189             :       try {
+    8190       64638 :         std::scoped_lock lock(mutex_tracker_list_);
+    8191             : 
+    8192             :         // active tracker => update and retrieve the command
+    8193       64638 :         tracker_command = tracker_list_[i]->update(uav_state, last_control_output);
+    8194             :       }
+    8195           0 :       catch (std::runtime_error& exrun) {
+    8196             : 
+    8197           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active tracker (%s)", _tracker_names_[active_tracker_idx].c_str());
+    8198           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8199           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active tracker");
+    8200             : 
+    8201           0 :         eland();
+    8202             :       }
+    8203             : 
+    8204             :     } else {
+    8205             : 
+    8206             :       try {
+    8207      324870 :         std::scoped_lock lock(mutex_tracker_list_);
+    8208             : 
+    8209             :         // nonactive tracker => just update without retrieving the command
+    8210      324870 :         tracker_list_[i]->update(uav_state, last_control_output);
+    8211             :       }
+    8212           0 :       catch (std::runtime_error& exrun) {
+    8213             : 
+    8214           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the tracker '%s'", _tracker_names_[i].c_str());
+    8215           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8216           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the tracker");
+    8217             : 
+    8218           0 :         eland();
+    8219             :       }
+    8220             :     }
+    8221             :   }
+    8222             : 
+    8223       64638 :   if (active_tracker_idx == _null_tracker_idx_) {
+    8224       12131 :     return;
+    8225             :   }
+    8226             : 
+    8227       52507 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8228             : 
+    8229      105014 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8230             : 
+    8231       52507 :     last_tracker_cmd_ = tracker_command;
+    8232             : 
+    8233             :     // | --------- fill in the potentially missing header --------- |
+    8234             : 
+    8235       52507 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8236           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8237             :     }
+    8238             : 
+    8239       52507 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8240           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8241             :     }
+    8242             : 
+    8243             :   } else {
+    8244             : 
+    8245           0 :     if (active_tracker_idx != _null_tracker_idx_) {
+    8246             : 
+    8247           0 :       if (active_tracker_idx == _ehover_tracker_idx_) {
+    8248             : 
+    8249           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the ehover tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8250             : 
+    8251           0 :         failsafe();
+    8252             : 
+    8253             :       } else {
+    8254             : 
+    8255           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8256             : 
+    8257           0 :         if (_tracker_error_action_ == ELAND_STR) {
+    8258           0 :           eland();
+    8259           0 :         } else if (_tracker_error_action_ == EHOVER_STR) {
+    8260           0 :           ehover();
+    8261             :         } else {
+    8262           0 :           failsafe();
+    8263             :         }
+    8264             :       }
+    8265             : 
+    8266             :     } else {
+    8267             : 
+    8268           0 :       std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8269             : 
+    8270           0 :       last_tracker_cmd_ = tracker_command;
+    8271             :     }
+    8272             :   }
+    8273             : }
+    8274             : 
+    8275             : //}
+    8276             : 
+    8277             : /* updateControllers() //{ */
+    8278             : 
+    8279       74351 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8280      148702 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8281      148702 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8282             : 
+    8283             :   // copy member variables
+    8284       74351 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8285       74351 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8286             : 
+    8287             :   // | ----------------- update the controllers ----------------- |
+    8288             : 
+    8289             :   // the trackers are not running
+    8290       74351 :   if (!last_tracker_cmd) {
+    8291             : 
+    8292       12131 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: tracker command is empty, giving controller just the uav_state");
+    8293             : 
+    8294             :     // give the controllers current uav state
+    8295             :     {
+    8296       24262 :       std::scoped_lock lock(mutex_controller_list_);
+    8297             : 
+    8298             :       // nonactive controller => just update without retrieving the command
+    8299       72786 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8300       60655 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8301             :       }
+    8302             :     }
+    8303             : 
+    8304       12131 :     return;
+    8305             :   }
+    8306             : 
+    8307      124440 :   Controller::ControlOutput control_output;
+    8308             : 
+    8309             :   // for each controller
+    8310      373320 :   for (size_t i = 0; i < controller_list_.size(); i++) {
+    8311             : 
+    8312      311100 :     if (i == active_controller_idx) {
+    8313             : 
+    8314             :       try {
+    8315       62220 :         std::scoped_lock lock(mutex_controller_list_);
+    8316             : 
+    8317             :         // active controller => update and retrieve the command
+    8318       62220 :         control_output = controller_list_[active_controller_idx]->updateActive(uav_state, last_tracker_cmd.value());
+    8319             :       }
+    8320           0 :       catch (std::runtime_error& exrun) {
+    8321             : 
+    8322           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the active controller (%s)", _controller_names_[active_controller_idx].c_str());
+    8323           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8324             : 
+    8325           0 :         if (eland_triggered_) {
+    8326             : 
+    8327           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering failsafe due to an exception in the active controller (eland is already active)");
+    8328           0 :           failsafe();
+    8329             : 
+    8330             :         } else {
+    8331             : 
+    8332           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland due to an exception in the active controller");
+    8333           0 :           eland();
+    8334             :         }
+    8335             :       }
+    8336             : 
+    8337             :     } else {
+    8338             : 
+    8339             :       try {
+    8340      497760 :         std::scoped_lock lock(mutex_controller_list_);
+    8341             : 
+    8342             :         // nonactive controller => just update without retrieving the command
+    8343      248880 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8344             :       }
+    8345           0 :       catch (std::runtime_error& exrun) {
+    8346             : 
+    8347           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_[i].c_str());
+    8348           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8349           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: triggering eland (somebody should notice this)");
+    8350             : 
+    8351           0 :         eland();
+    8352             :       }
+    8353             :     }
+    8354             :   }
+    8355             : 
+    8356             :   // normally, the active controller returns a valid command
+    8357       62220 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8358             : 
+    8359       62220 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8360             : 
+    8361             :     // but it can return an empty command, due to some critical internal error
+    8362             :     // which means we should trigger the failsafe landing
+    8363             :   } else {
+    8364             : 
+    8365             :     // only if the controller is still active, trigger escalating failsafe
+    8366             :     // if not active, we don't care, we should not ask the controller for
+    8367             :     // the result anyway -> this could mean a race condition occured
+    8368             :     // like it once happend during landing
+    8369           0 :     bool controller_status = false;
+    8370             : 
+    8371             :     {
+    8372           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8373             : 
+    8374           0 :       controller_status = controller_list_[active_controller_idx]->getStatus().active;
+    8375             :     }
+    8376             : 
+    8377           0 :     if (controller_status) {
+    8378             : 
+    8379           0 :       if (active_controller_idx_ == _failsafe_controller_idx_) {
+    8380             : 
+    8381           0 :         ROS_ERROR("[ControlManager]: failsafe controller returned empty command, disabling control");
+    8382             : 
+    8383           0 :         toggleOutput(false);
+    8384             : 
+    8385           0 :       } else if (active_controller_idx_ == _eland_controller_idx_) {
+    8386             : 
+    8387           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the emergency controller returned empty or invalid command");
+    8388             : 
+    8389           0 :         failsafe();
+    8390             : 
+    8391             :       } else {
+    8392             : 
+    8393           0 :         ROS_ERROR("[ControlManager]: triggering eland, the controller returned empty or invalid command");
+    8394             : 
+    8395           0 :         eland();
+    8396             :       }
+    8397             :     }
+    8398             :   }
+    8399             : }
+    8400             : 
+    8401             : //}
+    8402             : 
+    8403             : /* publish() //{ */
+    8404             : 
+    8405       74351 : void ControlManager::publish(void) {
+    8406      148702 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8407      148702 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8408             : 
+    8409             :   // copy member variables
+    8410       74351 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8411       74351 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8412       74351 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8413       74351 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8414       74351 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8415             : 
+    8416       74351 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8417             : 
+    8418             :   // --------------------------------------------------------------
+    8419             :   // |                 Publish the control command                |
+    8420             :   // --------------------------------------------------------------
+    8421             : 
+    8422       74351 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8423       74351 :   attitude_target.stamp = ros::Time::now();
+    8424             : 
+    8425       74351 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8426       74351 :   attitude_rate_target.stamp = ros::Time::now();
+    8427             : 
+    8428       74351 :   if (!output_enabled_) {
+    8429             : 
+    8430        6048 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8431             : 
+    8432       68303 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8433             : 
+    8434        6087 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8435             : 
+    8436             :     Controller::HwApiOutputVariant output =
+    8437       12174 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8438             : 
+    8439             :     {
+    8440       12174 :       std::scoped_lock lock(mutex_last_control_output_);
+    8441             : 
+    8442        6087 :       last_control_output_.control_output = output;
+    8443             :     }
+    8444             : 
+    8445        6087 :     control_output_publisher_.publish(output);
+    8446             : 
+    8447       62216 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8448             : 
+    8449           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8450             :                       _controller_names_[active_controller_idx].c_str());
+    8451             : 
+    8452             :     Controller::HwApiOutputVariant output =
+    8453           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8454             : 
+    8455           0 :     control_output_publisher_.publish(output);
+    8456             : 
+    8457       62216 :   } else if (last_control_output.control_output) {
+    8458             : 
+    8459       62216 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8460       62216 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8461             :     } else {
+    8462           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8463           0 :       return;
+    8464             :     }
+    8465             : 
+    8466             :   } else {
+    8467           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8468             :   }
+    8469             : 
+    8470             :   // | ----------- publish the controller diagnostics ----------- |
+    8471             : 
+    8472       74351 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8473             : 
+    8474             :   // | --------- publish the applied throttle and thrust -------- |
+    8475             : 
+    8476       74351 :   auto throttle = extractThrottle(last_control_output);
+    8477             : 
+    8478       74351 :   if (throttle) {
+    8479             : 
+    8480             :     {
+    8481       64982 :       std_msgs::Float64 msg;
+    8482       64982 :       msg.data = throttle.value();
+    8483       64982 :       ph_throttle_.publish(msg);
+    8484             :     }
+    8485             : 
+    8486       64982 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8487             : 
+    8488             :     {
+    8489       64982 :       std_msgs::Float64 msg;
+    8490       64982 :       msg.data = thrust;
+    8491       64982 :       ph_thrust_.publish(msg);
+    8492             :     }
+    8493             :   }
+    8494             : 
+    8495             :   // | ----------------- publish tracker command ---------------- |
+    8496             : 
+    8497       74351 :   if (last_tracker_cmd) {
+    8498       62216 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8499             :   }
+    8500             : 
+    8501             :   // | --------------- publish the odometry input --------------- |
+    8502             : 
+    8503       74351 :   if (last_control_output.control_output) {
+    8504             : 
+    8505      136760 :     mrs_msgs::EstimatorInput msg;
+    8506             : 
+    8507       68380 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8508       68380 :     msg.header.stamp    = ros::Time::now();
+    8509             : 
+    8510       68380 :     if (last_control_output.desired_unbiased_acceleration) {
+    8511       50929 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()[0];
+    8512       50929 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()[1];
+    8513       50929 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()[2];
+    8514             :     }
+    8515             : 
+    8516       68380 :     if (last_control_output.desired_heading_rate) {
+    8517       47747 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8518             :     }
+    8519             : 
+    8520       68380 :     if (last_control_output.desired_unbiased_acceleration) {
+    8521       50929 :       ph_mrs_odom_input_.publish(msg);
+    8522             :     }
+    8523             :   }
+    8524             : }
+    8525             : 
+    8526             : //}
+    8527             : 
+    8528             : /* deployParachute() //{ */
+    8529             : 
+    8530           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8531             :   // if not enabled, return false
+    8532           0 :   if (!_parachute_enabled_) {
+    8533             : 
+    8534           0 :     std::stringstream ss;
+    8535           0 :     ss << "can not deploy parachute, it is disabled";
+    8536           0 :     return std::tuple(false, ss.str());
+    8537             :   }
+    8538             : 
+    8539             :   // we can not disarm if the drone is not in offboard mode
+    8540             :   // this is super important!
+    8541           0 :   if (!isOffboard()) {
+    8542             : 
+    8543           0 :     std::stringstream ss;
+    8544           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8545           0 :     return std::tuple(false, ss.str());
+    8546             :   }
+    8547             : 
+    8548             :   // call the parachute service
+    8549           0 :   bool succ = parachuteSrv();
+    8550             : 
+    8551             :   // if the deployment was successful,
+    8552           0 :   if (succ) {
+    8553             : 
+    8554           0 :     arming(false);
+    8555             : 
+    8556           0 :     std::stringstream ss;
+    8557           0 :     ss << "parachute deployed";
+    8558             : 
+    8559           0 :     return std::tuple(true, ss.str());
+    8560             : 
+    8561             :   } else {
+    8562             : 
+    8563           0 :     std::stringstream ss;
+    8564           0 :     ss << "error during deployment of parachute";
+    8565             : 
+    8566           0 :     return std::tuple(false, ss.str());
+    8567             :   }
+    8568             : }
+    8569             : 
+    8570             : //}
+    8571             : 
+    8572             : /* velocityReferenceToReference() //{ */
+    8573             : 
+    8574         468 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8575         936 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8576         936 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8577         468 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8578             : 
+    8579         468 :   mrs_msgs::ReferenceStamped reference_out;
+    8580             : 
+    8581         468 :   reference_out.header = vel_reference.header;
+    8582             : 
+    8583         468 :   if (vel_reference.reference.use_heading) {
+    8584           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8585         468 :   } else if (vel_reference.reference.use_heading_rate) {
+    8586         468 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8587             :   } else {
+    8588           0 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8589             :   }
+    8590             : 
+    8591         468 :   if (vel_reference.reference.use_altitude) {
+    8592           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8593             :   } else {
+    8594             : 
+    8595         468 :     double stopping_time_z = 0;
+    8596             : 
+    8597         468 :     if (vel_reference.reference.velocity.x >= 0) {
+    8598         229 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8599             :     } else {
+    8600         239 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8601             :     }
+    8602             : 
+    8603         468 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8604             :   }
+    8605             : 
+    8606             :   {
+    8607         468 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8608         468 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8609             : 
+    8610         468 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8611         468 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8612             :   }
+    8613             : 
+    8614         936 :   return reference_out;
+    8615             : }
+    8616             : 
+    8617             : //}
+    8618             : 
+    8619             : /* publishControlReferenceOdom() //{ */
+    8620             : 
+    8621       74351 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8622             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8623       74351 :   if (!tracker_command || !control_output.control_output) {
+    8624       12135 :     return;
+    8625             :   }
+    8626             : 
+    8627      124432 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8628             : 
+    8629      124432 :   nav_msgs::Odometry msg;
+    8630             : 
+    8631       62216 :   msg.header = tracker_command->header;
+    8632             : 
+    8633       62216 :   if (tracker_command->use_position_horizontal) {
+    8634       62216 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8635       62216 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8636             :   } else {
+    8637           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8638           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8639             :   }
+    8640             : 
+    8641       62216 :   if (tracker_command->use_position_vertical) {
+    8642       62216 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8643             :   } else {
+    8644           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8645             :   }
+    8646             : 
+    8647             :   // transform the velocity in the reference to the child_frame
+    8648       62216 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8649             : 
+    8650       62216 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8651             : 
+    8652      124432 :     geometry_msgs::Vector3Stamped velocity;
+    8653       62216 :     velocity.header = tracker_command->header;
+    8654             : 
+    8655       62216 :     if (tracker_command->use_velocity_horizontal) {
+    8656       62216 :       velocity.vector.x = tracker_command->velocity.x;
+    8657       62216 :       velocity.vector.y = tracker_command->velocity.y;
+    8658             :     }
+    8659             : 
+    8660       62216 :     if (tracker_command->use_velocity_vertical) {
+    8661       62216 :       velocity.vector.z = tracker_command->velocity.z;
+    8662             :     }
+    8663             : 
+    8664      124432 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8665             : 
+    8666       62216 :     if (res) {
+    8667       62216 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8668       62216 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8669       62216 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8670             :     } else {
+    8671           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8672             :                          msg.child_frame_id.c_str());
+    8673             :     }
+    8674             :   }
+    8675             : 
+    8676             :   // fill in the orientation or heading
+    8677       62216 :   if (control_output.desired_orientation) {
+    8678       48924 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8679       13292 :   } else if (tracker_command->use_heading) {
+    8680       13292 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8681             :   }
+    8682             : 
+    8683             :   // fill in the attitude rate
+    8684       62216 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8685             : 
+    8686       43467 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8687             : 
+    8688       43467 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8689       43467 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8690       43467 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8691             :   }
+    8692             : 
+    8693       62216 :   ph_control_reference_odom_.publish(msg);
+    8694             : }
+    8695             : 
+    8696             : //}
+    8697             : 
+    8698             : /* initializeControlOutput() //{ */
+    8699             : 
+    8700         124 : void ControlManager::initializeControlOutput(void) {
+    8701         124 :   Controller::ControlOutput controller_output;
+    8702             : 
+    8703         124 :   controller_output.diagnostics.total_mass       = _uav_mass_;
+    8704         124 :   controller_output.diagnostics.mass_difference  = 0.0;
+    8705         124 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8706         124 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8707         124 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8708         124 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8709         124 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8710         124 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8711         124 :   controller_output.diagnostics.controller       = "none";
+    8712             : 
+    8713         124 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8714         124 : }
+    8715             : 
+    8716             : //}
+    8717             : 
+    8718             : }  // namespace control_manager
+    8719             : 
+    8720             : }  // namespace mrs_uav_managers
+    8721             : 
+    8722             : #include <pluginlib/class_list_macros.h>
+    8723          57 : 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..172ae76f65 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2201 @@ + + + + + + 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 + + +
+ 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..a90857500a619c25c8b6fe0f97b01b1c8604e3cd GIT binary patch literal 25810 zcmV)EK)}C=P)~d-1*RE}LUf5D^jZbU9xxm+9w! zr%~+(--h&3Lzw_8pb)T%)i_4lRDjdqu9bj2-R2lgV^jkQW2As0DH%rn7zM-}AlrR& z*Ag7pHBUH83)X$K>fNNK1QMU_O4}el&zUP`dwl0UT9^{Xh1{P9PK=yPAK$%x=piNO<|0^ z^9(VLz=bi68z#ayWT5-;u{zhRn2l5^GOqS9N;o-UhT0S|MwX8pMG|29IMo;>z-*i) zz_xL+7&z(^6WnUM@e12K9X-lKInj0ccM!%>Q3j{w-Qv=K%NG5x3Ezct2j8+E{V z-UfLdY5`1_X__=%t{I=kn?USjyx8z;g)> zuxFIO?IXBD<@V%=KG#oEYCvPx6G15gj_iAfnSo+m0vz5m>bi)1hpx{~BK^7r~NNiBa0d9XE=h?$z(J%WZTt4KZ0s7uG-JB(1M0X3}IPkM$qKaMjqug=ev zoii>jCE%pSco_z$ZP4Bt+r|caj1uc~h?d`_5`RBi7`{e&TU1^)czro*HZaJXPPS^B{i~lYn zlmG)f?U?~27-Js*5PanT;uQ7Wwd5lS~|GsRh4i?KzU zO+lfhKZEAlt?NImDKa(j^OacvY%5}(^F>7gx0SU4<|~x|zQ#7^Yc~QOVikg;#~5qC zB)C(y%l9-rrYPFAUqEYmQ!T~)8h$8YjDRC%6b-Nz!J;yvFv)6+V~pIiDfs_@K5bF$ zms$(ak?=bfQw}KT_24@#^*Uu3OGKz5I1$x>kC=@eTV$1wVH2wIF|5>_kC_PN81qH} z>ttA{#$M0!gdFLE`KXGY5;5Wvr+Hk`7{|E02zA?T(}nQl%qq8oygvFnC4ZCzc8>qo z349MG()}GUMptV3$P=RrP20XFZgV)<4ODA`NR;=AmahF$fQ&pRPIN4VgqQtB284H2 zHi6nEIEcmTgVT{y#<1<5HP`WsP?$$W#|Y*+h%5MNgzgFZ!H6-Q!~ekqwTzMZ5>kBu zDnDyoAy!D)`;8O&Y#WFPT4{o;Ij8I0FwTY}qN3~6k=EMVAkP!98P1#Roi91y<#Y~y zI`i`sc>z>4tzhk2xG;{m%YG&)0>$(C=S$ z$>4}l0v6(S2p|v31NO*qSSDw%y$JD7Fb2^mQDWC%@`2qDzTLCyaL}1N2hMQ}5dDSj zGC%Uu831|q_E3!u)OG3Zr46uj_tFMfy&uvvCK|7E!s9-Z*3B9GIP6ftZ0<$CCYmq_ zwS6W5(vd+x|AE+b8Q0^_#KfL1!M@uBn*t^T*&P}NoRoloj0(mC$^kmiVRn1mkZL|Q zI4A7bI6%*60dR_BOht_9#tW_&-KIs3R^hhigEP(8qkVp?u#PcZkoIZ{fGM1P!gmC4 z0(L7nFNfbKQ?0Q^s#JS=p$D*+>R{Z(?G~#oaSGvh8a+tiEO3F2i1tGTFo?rF6H!Kx zKsRb(9a6x;$~KKLoFgvs5~W=yWnUYk8gSAAdYnT6d2aH6b&TchLq)Z^VJqbv7h;c< z0tS0=SXn<m$v(Eh*Vk0E$0GZ4N7BCwd2PmdrWf=9uSg58W=pm-y z?_s;v0&-V(9H72yEyfI#2b7#w1W-TMeJK#gy;V3Na9#5l*)ggCl}o__nDa-%C=yj| z(d~Rm#x)Np_k%}~k1W#DIMQ^&3T{PVx+}cUj1i90?7gmC71YLvM6@(Q9#D<({ebEQ zrLpd_C%o`Yr|jGJLK87l`TSNTMh%F&C;_(md&26gAQ-@K4q7|Ln&9BBNh4my8lV~@ zKv@C2MEDwD8++VMW7FbNS^&Kpd(;5&Ko0#NP5z$f%RZOnm*F)Z?#Hno{+4-&+!!n7 z3>utdDzKEbYqRLk2onPx0Rw#ixxYq_5w5pFm=u=S4e-VK$zL22z~mc{G3v*N;!-Y` zIQQ**^yX2VjiL*IqVcMM+`hoT#^TS0DE%pJ0XT%g-ICern&&w zFFlmuGT@0W{O)_#Rz4PpIfK!iM2xmEx)GkkwTo~hZJF!yc?*Z>4;Z+Fh@Zj`voWe} z?S~8zUy_vM28=FFqY6-TtpkZsh>(gH+ybjAnr-Op6<*wt=EN<D#i5u!g zpqc>|h{+)Zz%vG&WcEx{o(p`$SO)--VUC*XtxZAg)d1BP^?=jGloTD${PkP_wrj%1 z;uzHoKWhQeKgfW1h?59N1+s`y*ENCeg_f@4 z_$#-2@q_P3ICEVb-|MH=3?ok+3&=ssfQ&ZdzVGpqG6Uy^*@uz8_$dQ+CUk-`3h>z2 zF`}bu(W4oU07EWa=yHt~Mh`LJG@OtLfPiiF#dQWljJ)shKZY@$OSo{4>M@QOWMEQ-u2Jb<`0lT#snBDDf$uz=%Cnnppp=;tolC!T6pcvH9-nHar&j7nK zrg5>9O<7HTaXI}J4k?V;+u2a2ozpvU3%fMyo=d#MoWO2F0pS;n@gG-O5pV+g2A>B<%&jlNJy!v=TAs-_V#d_5j%|GaM*3{5@q)l$evgxnGv~Y;Q?S z&*i;0bO?DjabsWrCmHZW*u)1)BFvPh2x9`QTmy0#m4F(I-C3-`vQNPbXCXk~(yHtS zrP^w6#2!TkMnLi#-S^Tq?+jv)2#7e@F*ejS)3P(=MQCRXX6m}pw<2TQA&TULj?JP( zY!oCPO9|rz&vgM%d!D)EJC*{lq@+&lhVDiLeD>T**oH5eSV@29^aip#w2s;)%^sRlHR@kF$MfHNni|9pMYF1$98I6mvJ)t}j7vBnyTbNOa{kckK=OSd>S+zm8pT28thJL)S@B zvJMNWYtnrq)y2NEuCd=4cAf2onPRgw!$G`to2X(;R0M?)eHT`YqX&tcv0$?<5|ax} zlk`voBV2Z=ny_2!d`wWb-a0PV0>%s?bgfpT zYK*aI0Vg#^CU%Ac97&9J3Pos5L!&RO+%bDO>Uo$(ju=DO$}o)XNv!lto&2tp{5aJC zU#dDsc^LFUC+4Gn(5s8;YyN0p{yLu#BWxwWHjMs#H4!ild=YymV#dBm$aD=kB?2xU zqvp{z9iYc3Nj!ODF`vyP0xBEAD2T__TBmTPQXGFi(z+h*7y5@Cc$0}T~FbIG@ z|E>enVqElHM^VcG+o%qbo3m1n@xsId?eJNo&=83_?1*LS{ko~}yW zY?;DDVrh`l7@3EOcO2uq6f9S1jGXS-laIwgN_=GOo*l+0Hqx~Sw`c*Y4`uU6%Yc}V zTK0saJ*?r6-{q0mGcz>vW1RKuVJz6Lu0i*)i)O$D{u}Aq#PHzYPQjFs{ZqJMFKgU1 zkWN5M*49kK5Lf(!!yI~&fy%!t5R+|(jPb#%j=pV}AKSZ9OaaRk6@-mgSUBHJa?l`uhL6$)jqjf96^@)t0#~@;j|{)qcGQ4A0Ts zhHOYkMaz~{23!I}ICWhw@)!}eZo#E(E{~CnL4$EaSqEHn7?Yz?Z>8uER=bWU-)r}v zBSTBqKiey?JhqRG-Wj}4&qxwqo4E5IFiML2{CMqb>#>y-fol?q*IWSOhfGcT%q;ll zI5pLoSy#7>{Mwm83|GN5=|?;G07Q*Z3moolpe zHtY%!`0ZW4bBsh&#qUfQi%QCk^5?5VD5;e9F(^2+I)#B*8k1|$0D?861{@>duIbD| zKZIda1Hh5QEXuBL+A-?c;{eAHqqdqu$K#X~1RORqv#RTx`t$(~KK*0F>aWlJz^G^Q zZSzRSB2P6ibxZ8gU-`0p6ag{lxTRoG&ARh-7(lm}WdR+(5HOo?8Ias?h=BA$t{+f8 z#tcw9g|QN1v#Btgk2I)4XRt3VRI`;P3yvbBx-T3;H5l#(3{9;(f~7E^YvR2g#>Z%KX))4=7<`-ow`P!+WpTa1`%1qJ zOSdW8DbxS)x=%Uz^I`R{`TM*xc~smOr;FB-(%#xbhuxs%N;V`BPQS$=8PfXfd)JIM ztdWe*6VuN}>ax>fw1=?z?87PM`FV)9gs|Smq)L#K`fa|A)d2uuA|N?19D(Jm!EP1ohH=fbv5*!=rC;Y6!@4AvSnsU34X6;$|Zd* zzImZ2XUu$a2&+doV=KK0BPxS%E-Vt*wSJ5R0oedwAL7i(W7JAdE)@%~Mi$gH4;Xk` z2Xqr@7m6$DCt)GkP5dg%1ZkIWH9QT0htc)=M?#ai}AQgPvbBx;{UvfPr1**#ec#Xu32Vb$> zN;We9Y_k9!+16j)G#_EFjGO2_`7@jJ6f@HtG2DLr2s?Wm>%sTy8&%C(d9?Zf&MApC z7BSpl_VWQaXX0yc z?Ex1JLUzrddq9^`g)RGEyJnp9c(H{gH~oJN($gyx)RDBeP%P+3Jys~(f2;e+C$Stw zC>W_|S1FxB!Q`>;Dij4p(JK@y))I%=T^lMZp^Y%@9eeu2VR}?~u2oG|Qq6IDZAz*j zk=;rvlgLBJBe^apo`zLzH!G!Pg^M$jtBOO+Ouq%-W;A@*e{nf|;d`Ti{fi5k0mqoa zrpr%UK+m-|Uw*QM(3GtfP*p0y#DR5Oeu^TO1tEflT=~0(43-NQ+RoP@D8^ZsV%Ne| z)t2l$3Kr%)a5EdpY1A>uO0YNBm z1<-#}1#Ae&$3BwqNX8kwT@g4rS~p)YHKG&=o?*zJl;vAxH!xu##sdz z1M-** zwp=0L!VxovbT>6I4Ko9hW1vQ5Gwt$DZw+0;wOInxVY~|_sk@P1-Cy4Q;2N?fdQ624 z8P3FKuSrf>x31nx*8c{mv1q!+$c|DR3>GhNGNmSQ59t)^Ks4q zzkDooy@&e&1|h0~=j3c054dOW+)H>=L5Mu7-!sMnJ9GDgz|Nfl{5*4@OrmeRHoI}H zCt0#T7XSqU{`OpeEXpnd9xoTb4g1gM0&G_gufNGER7$vbr9ti+%~u*=gy|=e&n4X8 zGb@#Z1?=jp32gzRDr^`~il~w|GW+9Y6=ahL9E9MEatS;iaJB z*eu%2kFX*=YW7H32$_=Ft(28~n`tEmlywYwgJ=Jf6UQ!jghG*zQJ#!`V%!Q$bo3B| zoCf`G{x6eaYDaNgly(7XhIdQ~*bJtp>bm0U^o~;8lt7 zss+?{jbIfFci?rvPmB?ZvkXYYUP(3Y-sg)4vVh9QDuBKR$MZXqn{eBV*dyQYCyW8` z#>~K#YKnz8P*}jlV-(MYwCgIM$0&Q);Ur&k2XTW1kcCO{r^#q4l-$xTCT<1sZ1Dqo zlj~u)z}R@i*(sHbJA)Ufu93c*fyB)+9(7y_Wxyrra2YVTN5vxEfhZjyn@mChOIy-HG=&Jr?}a*Iwh@dWrex48OxM~mE=oW> z)f$ZYnMt4n89rVfl^AWrr~&zuF22yX7Vx%c0o54GRs6vayQ=sT`%zSq)DEu!@`dCJ z#K`*DI>tE40-%;XK-ot%e+I|W)@o!5Zx&#WxDMuwkJ5tKQbh4L{OZpz>H&q-pD8In zARePla6<>oRL^*fOGPHSoPbjJ69g$@)C7kuosS$Srm!$b%@}8gQ9DNRS`1!UV~%@* zTH*E?R)m_*uJ{F*9%6W(L*n;sKR{vGC&=VM(gGH0Z7fENjWgp*8}!W#6CuIfrZAC3 zkC3Yj@6&$6%V~0Ih$lwPa5gNBtIe%E;~Kgi`i$?^8lO!$WsmdiS~3;>`p^H&7-$o( zguogM>2As?yhS-Esseb5laEIx1;8y=Zl}ZE84&C1QZQyx(?ub@`?zt^$g(}FCBTm^ zS3s%z;26-!@EfBd>3yN2XU2pufTb+99T+ZdmUUv5TXwsj-;zq?{5E2kP=((}%rG%~ zsU2aA@0=MtJ3KMW?66sE-0ZxU82S{GNX(6Bvy3I|?7VqqNM;j9%%UNNUp4O}W+^TO z^sp1n5Yx=dR{_+Cx#a_5yH+;AN6G-I?QnJ5#SZsY?s*Vy#>S0TmN&4po9vz*f*j-E6Zb`y5#%}l=bYc^nQPl z0P{owxu;$d%@>vZ^FA9UDn zu2U`ms(Tw|+;I9BUPR{v9lMw@hNh6bO9Y%>3~2k92U!4oFfapXt1-kE zk|sV<5MmPx_Jm-Yn9Y8~^z@IIuEUn=CKEI737f$RY1O#Q4NeYNS_sIzF4y{M-xcKS ztKYCI#7$vB+rnEcfTES#%y8i7F9K##+21v_fg3RvN{skU_9`)=#hBm5$MAzWU3vFl zMEmEW1&rWLUC*U6Y2RLmnH9op4S<8X_5_*$bzN_|V%l7Uv2MS6F87>W3#M1OUmrwG z5vnj&VpIFXOemsbq587v(Ct2M3ion7x6}b+yyEl0@fhWcnC${-c@dE*j93N8#2%VV zLRZ(q0EWEfjEy-)&M@8)P*3&DYh75^-iC1-q+G_X6(b%U8Sk`<*vu)Vs(4%Ar>o z#sFCD+5>XeE*v0xySD?Ns$d9U}Dbi<8ioF`_(foEL6=m6<$iUdQEq+Yx?S~zhb-RuOYX$1+-RkNVPXXIiOFbZ9F zjuE#PpvsACRZ=fCMQ^)-||$%8Zo z;~rz&ZwgfolH6Hvb7#nTOOp5L-LIcc#|nxKcQf9-`PM66MiNuBfkk>UEVo{ja&tim zbwCrwZE#DeBzY6na`@VURBFd!yPO z*4|R~QQZ8yd&tlm5~6j3_G1a-ctFb-f45LvzlP@5(4>HdHd;psmjw(lzbdH;m-;zc zQWcOwRwB)LNZD_5`$NYqK){>Jz&?id~!SHDskQc28PdpH$5sEyCz!K?iuP1AdM@ zL42Gj_(bUpV*U*?sC&TEbl*oz1xWBK#CPZh$Ia%Dc zvup5!0f(5|M&Za&oMhHE`Hj+}?mE;UxkMu&4y4gkKyOtB!(qUA2DhwEkL4Bsa6B

YbFWj!QcSZ7|Ho{j^a5dh5(G9`~3HA2BC<^%FA{G)F!8C zMRPQqzL~Dr;xo~qzF6Ff-=4(6RMpdK+^bt$^HNs!=1XL^ebf$+wHo6QFvi4)1H4+! zTz6mCm2YcI357=h_b<^08UA=|-|ltRIJGO+^s+A3?y^NW8cB*=2>bkjAk;=N+ zsz~A1laS6CpcDm zIU(%c73R!@wU&Q-DLrz@La$3&uU#ZmtINc$M zZOp28M`LFVphkfu;KXg^^GuW3qmwZ;Z{ZFmknd( zfW3zEHE0hv_C(b7(}q#qK-cv#j;=ks+pa+5v0cLNUwH(An9cy}lgQ&UI|Cq$>K41! zjPX_WvJvKARVzYJ4P|*n@6Q$^3T{DbKv4FZ5M1wq<{ozxL zW1GU6&y{X`pFcb}F{$2bPC20h5u5Qw9ESAy#sq4GJHNtvBpcPxF$^pZ`pGuN7{-`F z5qYVY!VPlrn6YD=`SJOm7yB4b(Ou?6Erh-y-s00on!>K2hV)!ayr<_T@XGehU{rU# z{h85{qs))Z!W+0P! zYy~ElrsaebY=2O07<|-QdqF5dV5ReSFamOq=bm!$;=a3&blzFx*{T(z4zP;xFn1pz zp3D6m9=B|a`t%kTO8iJxnD2lX!)%7yY|ECGTr=A_#tFpZB^(XsnaX7A&ok3G+~Kwz zAsBw|mn}W4?hF@55at+~oCa&}*XK6Rzb3a?PHk?=8-_G?F?_fWz*EhgfJru>mFn74 zu%hn4p;z06^g;HZP}fjQf}vW=o*fp~LUWX3Ky>32wml^7ekN-SdWO=45Qu{g>yo&T zQpO`ls5lc}Q`i#VZ%pyxh+vd;{%+nYK36JV7$g(sy?zbMWG@R&KGL9B3!;xE_K9+=<=lvb)5?L-04(r!YTH#4%>a4|bQehF((PiG9d` zF%N>lxNF8DnHqn`{xVlgsXNWjCUD%zDpW=8FbT$cCrg45TqH?u*L;>|VXA%)9<@)*&HW=Kv2Lv~hD4+2&(?wsW-nd8=gd`xtla8y^J6=QG- z(+4n2(vx8fNiJ>P>r*fR(lPlW82JadB;B!q*Qg3!`H?hz11os79Z8FsB%eJY@g&b! z<}Fmm^%8ONhg`U#KXV$ED{@)=xU?>x3~0ls0W@Izs1JZW4ZAN^|Mk~dz_KTB=#%XE zo|=c8np35Q%0fKJXAf`uxEo!2MtAjCm<5~)$HJEKZwZG4sK(fNLSiuPa6*#i5S||` zQ#kx0SINffNqNxRrJhu);7Vcq#zY%_3Kw|PY`|Cr{F@bbzXp5HHvB>J8kW5k{F%m# z?h{Co2o;9ToPLw;hS86UC9Rr-3xp5|W(t*!VqC*tkMM(!p$!i_KWU?~q8AEmKk|V} zUBvJM6ahJ6cCko9#BOlMPZJlRAS4)P0@NNU9nt}Esv8;Zp9YTt6lb&<<4eNXDKKNK zPULQLt(fe?YKljUQCb0D8^&RCtpVI=uFutF&EH?=Q{B9-rB!X=BRWBXu)fThi|;<+ z?vNLD)^*K^nTtDEQCE9SVWRUqW#rbUusIPiI!+)LGZL*+7{ccbOb}b{?-8d^IYyjF z2B$a0KE4_gD9J;%<;DGMR;wgTjiHw>bUs13 zz{uNXdW>lDA1fc2+Y^sV_O@1xkJpS;0qtXa{qUOKDj*VsGN1;dXUGrWKw{(*00w+K zWX-=T;CinDY8tQ2dY6KKv*Bk-3j3lF<7;{Ski5EB^Z^(X#|7p13`&Xtq}j_dMgj@} zCDjN3&BPSu(3Cf<-G=qj^Xnt`nRk51@yA+4OKtZ^Z-}}3v=$>Sq=;T$POI|UfO`Sn zDg_{$a&n8$HX3I}bJLyA4D({#(potr(A1YAde5!lq=wReD^e-Bv5S{0k1^ChTOE4K z5n%Ma%M}Tvuzqf_O7$*)w?Y`et*u~ERzW5g3a=7`JU|4vNoN&shX}SWF~6IbGCgUG z%N2(6yoZ<_YvT-imo(oN!iLbCYtQQtvf=e&%x|3E1{hQ&dKaLVJtTJ59EI#J#J!Cs ztru9n4h1t_s&zm*t>kr57xQ3efd=aA}>0og5&3zdsD zj3Zy6&t}`DgUuma+Y~z!*NhJ%ks|o7x!_8#e*d3>`+rBAk9smXt55d?estg^GWb$J z(p(vEc%EiFaY0{Tc1U}j;^4u|4S@r+c#vROzA`ads)t{Q;W~f6evHMk9KU3Yc?rff z`Bo1YDb2WZ_KQNsms|y^mE19~XV=)mK@}<()tvlDkECb*l4^eLqMB11M@-`6q;>c^OIZG~L~@&bbD=n7YUI9GQ$pJc75G?ToyO8Z)vWWyI7aeP z?Yx8n9y93IM)e7(+42HGkGp_6@t0V>`1QjUc!6RpMihU+Y;(n-W}gZiCnEtCuRalA zy=(m#)g^B-YC;7Tdc{_t0RevSf)+7c;wTf1Q(%vZ7*M#o2581u#@iM#RJy=!y@8BH z0jA6~e+{8mg)kQ@F;}Q;fc&-+bG-_;Gg?5H^ZIHXT?<#VR*Zl(0^^&<2<`zxIkAPQ zg1~mV{hICGH5Q+w^9==g1Tp?cUmBou3K_N1FOP|S^L{O0qyzm0I$S}_9>opfdmK@n z#j+*QD!Js(K5 zrtJA|@7SA8!`Zp^P0wG)GDP5I$fomk{QWT}Z0`E%Nu17)=ejmsY5b?xSB_)65zrYO%~K< zAYHkPk-fLJkI~KbIU1jFou(KB8+21Rz- z5tXw|g3%M17M)13a2o%ccfKl5Mt2T~DHkj$k7&)|azMm%0K5;NZf5STco^$7+VJ_m zZ5SW5XHM_@Hq5SyeE`LxG^H_;SMB_}b`Q!P%1j2MqN3#Z?L*&i(RmR+DZms?kXf*= zNmJ-{h&VB#=*F&<4mNLRmXGWAY!!+!Vdz@jN~Qn2C(iNnx5Q_=itejGQbI) z6L|J$Oy)NBR9Mu8FL7hgr9fK16lbhAhNhF|*Wme_Ub`|-k{CW`gWbNFtz*2B z8bdP`@K=dIlZ0Q%v)^!BwCmwx-20u}P)LfCaCO&IDw19fH)LmvP~5R;pSC8lt;m4* zKf;}6MWOUeVe#>ay;2(o-)T};N`Y!C zdnzny!UUQ(H9~09+MLk&hUjlEyL{_vv@%<3yGv9^}|1tn?GA?;v*?j zDgvr8@;QD$!n!c7(zB#_*GcDQfWw11SF%h6PWCNV$i_7RwqSH@l*Y)ikpbK@Y{x6` zw7#}4+jMxiV=K8>_{Eh!CfvTbq!`xj#T8OSn}h;l++yiP!Nua8B#8pJjW}F@p%LTP zhgJ!71c0MMfrmdM4*6Jq9r%uN8)c2f?_9vX2Y??h))o$> z!KoPYOESTomm-7gf?He%*C=L{OV?{j9+xQf0q`*LSPpc>?5US7FR-U+x&E;joAc0X zS7yJ?vz3;8_Uj#A=P2*j%RBB}m)=~`4C9s-lgAFVWv9m5tL`Mf7~NZVm>h+2)GPoc zd+^I3VyK8IJe2N)ZVQb^a#wK=$s;~V(!EgMM(gHls?C38R`e291_Wdh6)~W8X3|Dn z160M&TOum;Ev z5fjxP^pTJ|c33Tm_3c5bCHAPOW~^|MSG~j}XdTAV0W#EUf4Nl;bAXg`aUh$TRdfCz zn*V6UIEP<{%b-z*Zx|IWU#!EEz+_vPGmtc0pFZ~t)G=J2`BO|?^}9fJ!I%e0gzSPx zv|-nVcr%EyP^z{iUm=SoLSLB*m&Szd2~JRqm~wmGDMs$1Tjwx=Eesb^_a1@s?qAFS z1BxQG+Bg6uoIxn|U0m^4)9V<9b39;u36TX*{ca$8-Bc7WQms>n+;xS^af|Vh)iu*HnAToBj;4v&e0whXzrY7~lVG)u}C z(+IFqZ_m(m^*a1|xJ1OuWlS<+U7NWGm9I84YO_?|XN6ketIaBa@6=|E7Z6`~6Ey>8y5wPeOMs=Wf;?^S!v;qxC;d#9K` z9nMk`VBh0TJ$dY53Inh61>1u0!90IL5Sp_AtL1w_2Bo=>^avN%O|M^m^lmRVW|0VCtU}tm9*Bt)! zc>1xRC|iqyqSfWA>TiEoQlL~(DGg(+E*cA_D9QHcF3SpHc5zwytv{ zmmI}*+CKi0emsx!T)&#AA8}aa9<^up(2u+_3tH^v6+@4yAORXMYIM}sRH$Ku7db-~ zL*XI^`F0QIYASh>_j5G`O)tXcC76WM${t6l_qlQ6Y0^>D%F(KpVA5kuIp>vE$#fVS-gbdD z5?iT(@QxOVSOma~_YHtrJ}QsH+YMy&<++gbNBAOMh^+9cvRpanqgRzz65CVzM` z&{k-q+Wq0_6UKSsMzbl>#YXVt}bzL#w2M>^80>23&97d9$-EXV>v2-_JqZG%e zCuRpLWkCuD2z-wdP_t%K+Odf}MK$_AFW0fjsjB7rYHiE4Qo1~DGtwIjVYyliqz@$n z^*uN)R7}$`94s2P5^)no8qh?W8KcU+CN{<0y=e2y4>y_dA!VuPb9}DIh`Vmp4kbP! zR!AkuPU9=P<8s$UhU*%4Q^Cq)0g%?R8~)%p+>oRSgRbicIC4Y!OWOwxIqk%_@Agpy z+{N14>pHdaB0`UWbOUqHg>jfgdJ50~!rH+TlON08&oYY}11{lmwnR+gIx9)W`)l^e z06b`;gjXF+O>V_!`r{FER0ljP_ts#g-;sQQG+QXxZk`!ch1zzEQ7>^p4a_5r+ldiWOOC{hH#c{8KZ;xv`SbL$;?-Y)EAu6a&wfIPuiWY-5122P=!<8y8 zt8FbXTs6uD>GAs#w-=eEXD}bp%W2%XYmR`>97e387}bDF-MxMVp+){KBM%^Uw^$GC zLe{)Wrtmd?)P?+)jsVfu`wb+|0M((FFY*(UN?x+qq~- zcmRc~^>rM<#qGGkA$Lea#hg(Rlh{%LY{2-8+Y&QWNX*ZVKgW3$#xaSp3LX-`GKc=( zjGVz%T&b)hG#}cGxSHDg+!hx}tQnfxt}UhhfWMC8zOn1O=XlLJvtUfQjj80FXV_zn zf?){+GISkPfEsX&YF2JEpFEP*g~zeYyj-iMy-gl@;QIB)*f42<^Q#$2A3ESEh(%4Ef3Xv zvaSM6${&ESg3AnO>H04U?x5h^T4nbj!3P!G*xE6E1$RJW1h+E^sV=yWzaY4I{G%G< zS8xY3PUH4TZe4I4z`+H#`3Ue8+#xL&++I_+Cb-Zsq-FT;6Wkt*U%?&F#)m5_H12T( zR~=?cu7<$J2O7DN>WBPHZNpi?53+XW6J2&OF+}(U1gN@zP>fO6_A8_tbESj;U#si$ zL%CAUJz0l4NS%*2Kd~0%{(n$Xxhr&>k_yVts-$90S`9Fn6Vfk0K;6tJ#;9w1E2;Gv zim+vrf0b0F=iM!-{>0(0C6$(STIpd(11+i5%ttrKuae>$wxlFG>Z_!xJs+UXuaKQ665gY`UJ1z zax3<`T+4t*Tdre5UF7A8uYdkr%<$mEFPnrX$0z8Q0$RHMlM+5g%=bOy)+HP`d=Fh5 z>n`wdB_D7cDX2N&BQaw86r#ejn%yrtjBcPsYR?gs;504S{g8_k@MZj>K;=Gl) zpdmm-u2fylPM^(edO~Hk3Zu1I&41U%?KX111z^X_HUB&pc#*|__}aL-H|<^$!j?gD z%mfe1$#rTMo8Z2yZeb29)h+bD2LZLj9Hed`vs3eQzqZ`!5*Qu$ao(Qn~;;_L~P_G%KpS$1x_#}Sr4^yq$ zHQVP}ayDmxcfr}5OON0h-Um=mwZ_@JVGKW(Ci?}bn(UL_8s5<}{0woM!o9-tPdR+g zAs*IQXjA;OeQWRVXt@CYQ<%gv&=6NK$5~jyGpuR>P}%r<*K+mHmiR0=&z?iC8G;CjR2`sBK<+>#zp%m&EXh|5n5 zsBZu3Evavn*I>XUN%VdSfLf0x^R%q*fCnr*ULF7qUFUD2gVuyG;u-{K>H07Gns=;5 z26wOT&ZX?SLik~Ku0BEghV6`*>(6dTX;--2(uI7Q2Rp`_AKW%JX&SA7w?u6P5JtJM zNZ~e|_{Rb&+v)0}7Jl})8P7=mk{>f)%n){Z!T2w4ntN741iR*U(^Pg{q5L45rk66{ z0X+v#Xs7FQM0J5zD{jMc3bFcb!{zpC&V};cmA$;X^<2jYhgil4iC*3eh=WrX0BGQ& zc*w{@1@ZK5sV_bT>X8B=%eaghMQ2gha1z2E=)%D*!a` z@p~&J1FG8*>Pc0dF0(b|tT*`dZCpz@u5sZz@Olutvpyt>(bF_gT$@6B}SCXW^CgYzmLPee|6^ z*fAow*H`w_#}ocSKVXd*a{5RN_?|w>fQLJM3~@KtwvUh=^xZzn?Yn(EzuQL{@b%Tl zw~s1d*gkS%lGknN+`rUZz8??TEe06-;1^BAXr(Ky}>>pYveSNGQ7p7oyIatW5kT1|!ff-PHm zb=m8WP&8)+e}R-`vaZ33hLBO?`hRpffL#i2tByC54}t1uAoU^SbZ1mUTLjYfNvi)^ z(L1rA9Q^jf1?3*K8gZ=j{LVEau(-KS+{q=7*Po117nHl-VFBc%rQ8yHI92tamEC{o za{bncs{rb$*4K$+#pkg;L%Hpj5HJCgBy8I?GbY=i<5!`^W3UC)f2so=yag!ZD$4P^x%SbG9vo z@N3_r6f2L~CfjECIE9Z=;UvX+xej2P^==0KYK1oYd4^Ib1PM4zW+0`D96*e9>04{J5mH@d$xHq7Z;a6eN%!G8SvagG{ z#V3`FVfwHE+!yj)L0AixT|w7Q>Lb8aI3UrS|6Kre%W$VH&Mntagjxb@T&`l!bbMc7 z`>C*C5yl@tnLVw5qzQnT^9byVz3ZB;>hHSVdtGM_)(C)0rh-4cT%8ka@)qBZsmGO$ z7o0sDF-oDxF@jZF2Bb@8G0LNKT|lX6CMG@*wl8QywRvCZxc_v*VYO2i+$sd%+d+*EA;@V%*E)^*rFzBd&L zjJQzrfU62DRSU5>(hu9>%1$Ryu9282+Xp?GfBiP)0l~vB?-ZgH4hgT1=k3(cC6)or z3(^;=2!Fh6Yc^FGJhv3g8gGXqTaWr= zu$UoH1uKV?e=`Vf*0^=icjk&-La^^JCKpIDz>ofpeyQqOP(JgV)Q{B|1{y@r1dSMT zqhi??&_2f8or`R<7L5Gx0-)Ca*tQDgF?1|0p%zP*8%F{(Via{r-xv$$+-8;XBUt=K zOH>M9d|^M80-{J5eaDTF)pxs&5lk<4P@z-k0qb)eFv@DL5#vBWD&}FP5}>Z_ynV>4 zx&b4z{@wv-8l#M=593~ZWM7Wi2hcP|4IkgIX^Lai5|d$Mk?#_=GupHl1M*4ygT;*# zkno-+z(+Mka+}y~x`NxirUMqo#~m)j(xR(`NhFs*!5e3kaMAyOLSoc_`mVE`Yi3w# z(>?ilFXxiQjRU=QA&_&*9|U+AKYY;(_n2ocm2FyU z(Z8PnEB<}k;_`EIK7*-wgIEOiJZU4VHW2fym38re^;KgYBOK&|D=%Yb_{HB&Owojn zx0bGOLnQhkl&11JDeGD?2FzoOZ#IdO9)3d@U0EIrfE(4(ZT1SeiWRfW4EGqZJHvON8Uo>?4vt9eLSV`hX7a&e=wgue$1lDGYL5?P*l)hs~J!oH3)ZN zVAunj1Xwwosjj6?A6znNB}g$>)}=s=bstRnNV(vTaLCn21S;a%IK1Q=aBC6+!`02&Ct}L!hKuaCdQ=MV&h_ zsy8f!>^@a|2=;>sojc#VQn3Y8Ga&vUI?5iUJf4WLTo_}%uI44T+f$(*6pjCUI3vVK z=IhU{)W-J)dIleoB=&P?!{!1JX4d(9Dl70A`w#kFtY8IKv+l7(3bX zxx6@k{eV=Mzh%RdB!(ct7P>A1`8FypCGp-K)+naqd0k))1OnOmg-{L)AmBJG_^{-F zOv_8Plo<#BbyV+~Ht1~%<}C17pSev@NE_5CQipMUIi!oa7XlWYabb*FjJQcoz?>a? z<+;%ztgNP(n9^iEZ0BXK&rn}btKE2Yj>Nya+}2fE3%wY54VhsS79wcIt`+*s7Fi&9 zuShhv3`hl6Cyx$ea)(RDd~!is@ipU<3b5%wYO?XlSFyfqs&A|+MN(D`p4%j6-WDm4 zp;x&7_EnO{Ot7SbcrJpxm7+QNY~0auaWI^^;gtN%cD;o^;6EMZiQcgfh%hg_3z;NJ z?l5je;oc>lxoRtRe|OjtD5nd<6f=zIUhq!|@IYcV^r0H2WFaFMHimmd?DOh981Y<| z0fEr87i;_cNfTVs$zAg~KE!}5oM&*`!NI0pTx$00S`cK=`eJL&^u@4ivkTmjOQD)~ z)ZvvHLSVVZbiD4nk0|<}ae!uwaI6g4CXDckq|)^))k^MGN?g7#`Suxxg<*JZN#_mHm-U?BmWA z{GsBORTOM6xjtyHXN^WbsOS3QCw8gI9_m_sV%K)PV-PZ}k9iU6X(KuIq9eZ@PkYU3phZv=uVjOx<5oZU@^?$vthq>6f zb^Gr(^LK6E&un{9Jd^25PF0 z%P!aayOS?@qrkXBs)P?;kxL-?u9Xe)vG@{QuPeT*+Dv*W< z8anvUOMWTJOujEHvnQG^r8rV#-yDoRQ(1y)(i3PehS@?1Iw*St2~sbvN=1d3vt|i( z3%^*tv)nNkAXVcQR##Peyl51q2d=IoN>7M}YG99FI5p{s#WW5fJ%Q@NTNYpnYmx45 z3hUS)Mg^c&C=QTQ1&rZQ$2itI)0b6=rkZ>5XihJ&z(hFoI!dM~ig!M6jQC$>uCQdO zYjnYJ6j!_try7OvmPti#rLXs#XFL{~T@c8TlfNCHQTK#jECBvV-80OCgi3eeIQvlM zVKwfB0l|tkQ&)p#MJe=pjAV47Bnr;iG-M)!poh)%De^}Nj3A4xh@b3Lh;Jy8QBhL5jt5Q@n>{UB57cg0%y>>D|83eUw8yQ<{nJ(YCerVvfz z@0`Ll{^Z|xJ<=4GXAn$baRzHsm=@yiokEny{`;rUkFegg^4w!M`0TN}LO*vb-rgQ< z63xPZvVX|ZP3}y7YBSKJ!(O7n(m(+f+cVE43vfJOU6M#yI~n`(|B$G_h+ zMmmq-{P*b$F@=C1COYh^dt^X$`|6$wdz88-Okw?T&^@N`&#vnMOM8##up_K%VFw~> z^V1s=GZ|Ux(oUgnY|*>+P~csP-qE8J){O46weHN&GnaaW?-3mC3mjDyMG2Tn&}b4b89YXimb_(cf%OR7ed1u_988Yw#@h} z6Nv-dzN|bg+XNU}A2Egfx(%aFHQ$xay|-+dQb7K$H4D=HTBsk=y$Ttb4AZ#FD%{=$ z^kY9eeS)fK_!u3Oy+7n7mTM-ty%`S*+c0<;q)xRJP?qmLhV!bz zhEby`=<7wIs<1ptq+MKit^@TPF&`@uK@O0hMt7E-N>qh>RYtMWE zk*(_HDO5M5Y1WpVS~ATdV9nxc29#HrFS*@=GUFNC6i)6%qm(vOQCIW3IxlVemTYG>(OKK>j8cKR`9 zbhyPzpS7@srhB#U5MH5PJs+(Y(|6pPmayxnaF_vjs>wgFuV9$Q_^9KvVRVtYEBoAF zdSTEUj~h`)ak=3jUVm?R{6U<&oX$T3eYa5Uj--=6#!XS3a$O_#kO_@Hn;wFVZ+_u- zt=N^F;M^gaWe<{Zw@lZxxZzvpKR>`k6JCgH-fMO>Kk#DAAKef-H$M6cI~0_!^%-GG zM?OgF!yo_~U~)_+d29yKUA4S6z|oIT>zk8D@N6L4MIa^3aEU!iV&H}IG?9Xb+v+%5 z%%AeSgq#R6*G~wReu!fn(RF;TuVWP!U(}}Ro@px)T)GHWj8XHjZ3W{K1F9ul_54h! zXUKpZ97A_{Unu0he>gWK?g_3F-b^t8W`>XSG?vVMT?Mfm0tbkAngn;BVjGCzqV$v)?krVLOsSm2{T*!T^Mth&)&xoUU57zx+goH#TSg#3@@=eKBjC_QV&rQFrXSEp<6&~ z6*KpzZ<((rQXM%01dTMCWq$&-RQY#&Uk#1!iR`%MOk~7JBs);Ov>*4l@kPK7o#O79 zHse)km-K{(W~7gK18Q6@<*u6n_i*lWQ#*yGOtcvmIW% zySDN1ey<<*qG$N2$Qsz)qv_e%M7}GtR_!cY~WAUm| zxXXRyrw>_XQ`=##Ph`W6MW`6_no=zuDm1j1uTv-4Bg~9ZRswgnX!OJCOU~wF)OL-v-YQaVB4)aPcAnyC9S4CkPA=9JLRE8S$pzWp z?_rQBBrAsx+4PK$LI!@1@(^E&IWybB7q6iJC!ntDZ7Fc;=B53~%w(1|@4enh?bXp9 zMD5W>meCUGEqgnpP<^4Q!UB`Lwj7@&q{ft!fkwZE(Eo}k8FE`vK2H7Oav)=0G?fYh z?p_*0O2o2e?^iRRLOQSsW2G{uVT|N0rrC+?LWYe23w^T~=kU6Q063RQECRr#`08I1 z)r%}-ro3UbnCYhaJqexZkHtTPvlV|i!sDR9u{REJ+{c6Ege(TQU(>uMY172{A&-dj z5#yPx#&C1j?id6k-8;Ij907!`<7Yp3gSBUT<>dJ+`wIH*uU8Hw0d!w-oU(RZx;S&q z?o6dlE9#Ondg8KM-&{;gjv-ZYfxg|-)U2gd<9vH-Jon~|U)?`9v**DIfdMfJGpcRu(iHb2z&a9lx+kQ)N)A1 zX$sP=E3-Wu8lh5CeP*-_-=~({9E6E`M{qHZ(dC?kc$_IZKypb}K~^-H+bd+sd^T#OxN{s%#Ro6d}gN+ITTLgPXGp zlNI#U+?-H)tWvGjfEq6m=>(XBuAzE_!E< zQ95X}p90w|#KwgS?zlE?m!G1~#_jV{Xe$CWnXX!6`0v@ckMKDwW%ajgT+Q`E8^g=l zK#lBKQ>yp+OphHqBkt3W9Na!@hu6pWy9^|Ev`^;I*AB0>UulQuWBh$$hR=J4u)|>t zf_G!3n&3?_2nNeO7%@ShMCnm?Jy&)Zt5DDzUumH27>A5;j)51&O0mi#l98B~hS-DQ zgKor!-@2UW`uCd)rzuqH-)g0ZNgNPx&$74S6FWP{IlGRjPU{GULV$^D2gOx325! zm5C&MJ%$l}@crbWz(O;<0;S{5RVF)(+DP@Q#jN~usExsl26i1~0rwiaR-FQx=DK*` zYz+G@jgiABg?+1Z z7@Uc$93r)($Pkn1KtnkY;4`JIezqz{7Std-I(Q(*&}E=^2(M`NHf4580WA3 zzLdPz_>X2^^9b~i02;rZsQu9F3+-?#O8i@ ztZ7}n%!9|8)M^V>o)(yu#Hr#KCqAmzv;k;=jlN=^Bo! z3~V&!_l9pU{DG5T^LxY9r)_idIw^__aF_FSBzwTM^yzoVV@dmrkjE^|{~hu;fS5jc z%&Gp$V~H4abuN!lP0aPG+p&je+^jUl8zh`lx$o7u8P#72FA;M@37>E7C_9VgJ4Q-4 zIlTIlk56uyH=p()=GtfPt8840F{}dos0LX9uo!dIN--9zHL9MKuwksCyPL9IeutP1^i$+IsVZjj1?^WTD(xW7 z0FxPk{TO#(&--lLXNXyL7u0PwZV~VpHZCkv z9kQgr8}bbpM_bK!?%3xaHlyW}rT+fUqIcW+H$N_4Xbr)|-|Bc0G2iNVV%ynSFeJ12WB^{Hnbg#;@Abv*)Y!2u5V>jnI2o1O>JCqxZgQFUd56@~if07{6*y z&z`T^BN&0&8&fV0iy*7^R#)HbtM-CSGbndZd*90LW7_?z&4#pHoq+!NX5W@m{}Hq4%P7u$%-wGgS$nD_CDn3;dtGA};ezZc^zJiz20Uz%Y# z!KEpSt+;4xS!}h-Su4g6TezjhZTxej&Or&!^Hy1Dm)OF&KtvNUs?_*ajE+rQ+BpR* zSKDDRl8EL0G1A%rHuhu~naTk>1Q)9<(tAvSdbsyQAEBVlBb=_b4|^0X8BNOx(Bk&A z2dvNYSi_SH`3XrZJhgKfJ*)-LTpui3dXNPxgnGIi1gJRi$-kJl;C39-K9i7@0nHf8 z?*~}(UAgqp_Ne%cQ5rxsM!dVqHZD~uzA}C)pXAM{537_+Q9Qia7o)Ye(}z}59Oeni z!R0YE(YUhxm@UZcqKQpd3DaqJ}wVDF1zAGfhHJ^=58!_sVA3aN&z=qklR0ctSb;wyyCt>6F^ z6TV9|GQycqns#wM=h^%2?I)yixlNND9*W3bF5wjeS9mgewsm-EHLiqjociE>osC8B zGI<<2#wcua)H{nWTh@W`#!aRr9oXxN=nJ@{mz&Nly5eVyn|RI(sa7`Qxo<2Iv0#)L zAmcXcVoz($)Fq72Z8hO%=DI)@v(}oa{FcoGA;OdYBbwBfw80(nGOA1#{&l)hfV(EDSP&?Oc59;!7F>VCmQm8E8VjSbH1&NCX)3)#@^0pFHQcv3O zT`vOsWcbq!9#v*ed6cYPj#J&7%7^^xK&wpEy3zb!08%d2ErIPMA}^bW;v%$^YL zT4oQDUgpFe?&eeNvmoO05)auo_JqV#BfR1}q{3qwI~>GK59$(0-A0>_QQ1zrKBfa$ zNe?NuBPmnXk!cNuRVuM=7X2tMSWt)g!KZ043pM~&7px__VDuHl^$OPEyhKAOx*y;> z;+l16I{4_8%@y{|mw#LAU8~+XLS2WxnEI0&phm0MW`}>XWnx}rAF2X!s^d`;+0ZO| z@SFhlSY{@84A0OneNJf>lkYl7gS6AwqCO?ELmc!ig^|@~J9I6GZu=Oo0-i36fPH=E z8E%YRGYW-9S%lvZI38xzDvu|HvY=guzNoSYfK~BP6K!pAb-~3or6cI%iCN&0gMT-6 z^x}%=nP0QGsN?`Z#TboEG1q0P+h+#8LMMge4W}CSOy(`?T64u_>zY}mbPW29-C29} zH@}>u(JNfv*oo$wf~2Gq~Ml{^8|m_FM{yB^W_z~?(_`juafB*uJ>qa0mplWzmD@_8~oV0 zwsBN*a*+WtZ`2ss;}mJX`yu@5^G~=R!fzkO{g6ovBNc|_+G+RX7}Z^i;#~q<&^n-i zJBQD63)M{ZO=aR9%!4#6pM$M;!9ItQWPp>Q*nWctN$fO6b5}3d95g;(%P*K4i7B=y zG|*x`#~}ao9RO5f)Hbf>ZB+i*&ZSb#+m>H6Si=-e>V8)-8(l2IKy@>OU60w~ z?gvo-6}f%JcnZ@PHGsd~nHoh0#Nb>u1DW0-ai@2!hYbKEb}o=Gj;Jro7-j!q#G3M|*icT~{f3^w=p%TvVYr_Yo6(}SlOPowWT zT(w9se)l^J`ILoe?wJ4p1UyMZK~$^NS~etgXp!WnXzlvW`6-yTPl;pT&@7KpPYmOT z%lc^6xKCJg<^eQhE5-_-7Ng`WM0EF39rl7$-uX{^hM#D+1V-uK+6#~4EgV=MJNFg} z{wAfj5CZ=$o_sh$y~ z5_`DNbpY7Pa49U7j&Ww8j9n|smgzB;3uI>%FBBM2$qBi5*oZMk{IjhB^p8N|b`~7^ zFX$W7W-x+Rvvl3<4`+%SDEY{QNiDZ*Ih>RydsC$bj*DHP>7tk+r>!l^mzX@j&Px~K z$nJScZN1y#_LTA{^$GGmo>q&L9Y1TE=`fO+No%Hku7wmx8Y#s}IM+_DFpf$nN>LK7 zq};L^wwoEFj)HFt^RwB}{ zW(%P9IIzhxY~PTIujB+k+2A37w+h^D8`4}VV;MDtuIvLpGmEhUO&I<9;$;{BUax}=XQgThq=p9m zH{sWI0o*~G@dgqT_FW&~&uqMLjBY4}nYqaY<2V7GMj7J>#AhDn_G1lE@u3); z4~&+q#1p!HkfS85kF5Ifm5fuSR?~wk2=wWSKp_awUEWQhqQ=YBNhUj9{*^>2F}xhw z{2qq;OBj45_~{Z%K3K7wP`X`d#tXm^f_s9MvE0%vOkrWI*t?EXI3A-?D2nsk(Dfts zE|O)KnH?X3RkDXxxuCz)$etmK>sWiCH>bO69AK+NGFNb#zjQNnb57}T6$bjQhC)?D z){?SgER+LjywU);8NfnXTxd>kCI2JEo1Y5>XTVA6EL4qgYkO9`!~`kENK+s^Cr_$G zDccxfWtDjrt@qC3-YaaI!I&mlcuvbdRHO6^TQKT~vGI}6Jm)%u_{BZuv%m`s1&G?z| zs<#5@N|P>ijJNzWG6YcF^+x~$AIHti)_3^e{kZBiqwaeAnwjD&p2VCUg>gVl;t*W} h8w)5+3~aBt`3J4q(;DyBD#HK(002ovPDHLkV1ld(z+?ab literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/control_manager/index-detail-sort-f.html b/mrs_uav_managers/src/control_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..1b4e9ff209 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/index-detail-sort-f.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:2185369959.1 %
Date:2024-01-07 21:47:14Functions:8112266.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
58.6%58.6%
+ 		58.6 %2142 / 365662.7 %69 / 110
<unnamed>58.6 %2142 / 365662.7 %69 / 110
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..bcbd74d7ee --- /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:2185369959.1 %
Date:2024-01-07 21:47:14Functions:8112266.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
58.6%58.6%
+ 		58.6 %2142 / 365662.7 %69 / 110
<unnamed>58.6 %2142 / 365662.7 %69 / 110
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..227459e906 --- /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:2185369959.1 %
Date:2024-01-07 21:47:14Functions:8112266.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
58.6%58.6%
+ 		58.6 %2142 / 365662.7 %69 / 110
<unnamed>58.6 %2142 / 365662.7 %69 / 110
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..3073302bb7 --- /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:2185369959.1 %
Date:2024-01-07 21:47:14Functions:8112266.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
58.6%58.6%
+ 		58.6 %2142 / 365662.7 %69 / 110
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..b6854ff552 --- /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:2185369959.1 %
Date:2024-01-07 21:47:14Functions:8112266.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
58.6%58.6%
+ 		58.6 %2142 / 365662.7 %69 / 110
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..a33516ee33 --- /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:2185369959.1 %
Date:2024-01-07 21:47:14Functions:8112266.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
control_manager.cpp +
58.6%58.6%
+ 		58.6 %2142 / 365662.7 %69 / 110
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..79bda55f31 --- /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-01-07 21:47:14Functions: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&)57
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()57
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)442
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3752
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)49554
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&)68303
+
+
+ + + +
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..d694ef8359 --- /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-01-07 21:47:14Functions: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&)3752
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7825
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)488
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)442
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)49554
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3774
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1025
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)483
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)960
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&)68303
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)57
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()57
+
+
+ + + +
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..4c6f787362 --- /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-01-07 21:47:14Functions: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          57 : OutputPublisher::OutputPublisher() {
+      10          57 : }
+      11             : 
+      12          57 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14          57 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15          57 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16          57 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17          57 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18          57 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19          57 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20          57 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21          57 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22          57 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23          57 : }
+      24             : 
+      25       68303 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27       68303 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28       68303 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32        3752 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33        3752 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34        3752 : }
+      35             : 
+      36        3774 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37        3774 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38        3774 : }
+      39             : 
+      40       49554 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41       49554 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42       49554 : }
+      43             : 
+      44        7825 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        7825 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        7825 : }
+      47             : 
+      48         960 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49         960 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50         960 : }
+      51             : 
+      52        1025 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53        1025 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54        1025 : }
+      55             : 
+      56         483 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57         483 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58         483 : }
+      59             : 
+      60         442 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61         442 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62         442 : }
+      63             : 
+      64         488 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65         488 :   ph_hw_api_position_cmd_.publish(msg);
+      66         488 : }
+      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..dcf855112a --- /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:36552869.1 %
Date:2024-01-07 21:47:14Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::EstimationManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()4
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)4
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)4
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)57
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)57
mrs_uav_managers::estimation_manager::EstimationManager::onInit()57
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()57
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)79
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)154
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const154
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const308
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const1732
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const9677
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)10380
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)106714
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const106714
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)106716
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const117091
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const223838
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const988372
+
+
+ + + +
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..7be34820f3 --- /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:36552869.1 %
Date:2024-01-07 21:47:14Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5bfab0f01d1938073440cc9dbc0542ded3c5041a GIT binary patch literal 4278 zcmV;n5J~TeP)Rn>+2gogp7QeMD~;xcY0OBgq0zOzG`c<)zKes#MIPkH z0Agl_KnwoVjWq!zFvssbxt1Beglo#yTrm~w?REo>&%VfOn}BSq$=S@y7tK3TX<736 zR7f9mny#cT-m>I0irUj-tfw$;C1&P~jYlr?3X6Ns^qZYJc8t&G^X1wWeyr~w>+!O> z5BIhM?rnYLZF_>|F}~rRdV2$I1rQHt*B+o01(Q-Ep2YJeQGAF8isa?LwmCVj?z8n3 z09PV55u-^w7u$;a9jdM@$wQ85eFf8&EVNF06#01RYy;+v)3(kvDd<1JTj{PC`<+Nt;6de zYX!c4YM2V%o%>ZI^Gs`@xDG~U;-a~F7lQf9l_RrZjMoX=BQyb;{F%0?pe+mhRG<^# zJ)1h%p7K;k+S6w$c)^4>l6XEoXVgi<&zbO=KDczwxX*sqoOuF)uSFWEz?rXHI%hD( z-zM;eRe=%t#YXD+Lj?YWJ55EBaHqLNSwiOh`=6r}KZhCBru-acbkmqI11Di(Y=8Tn zj-L%^Tpc7$9!2!KBv-)}S9DHX(MLgm_eFuL=(?Wk;3>NQ%i%f#2h_*SXlOVmwzBKF z?3`7^aWBus4X~!+rsU9VL7Wi17Nyt1^<73n#A9dLnj#J2Ino|lRu=$Xl!3+Az$j^e zlNB(rbB5T6&{TkF#OQIVnWiBmo66InbFxWJz2cFQLkXXh5`*~>g9mOx(j=?$>k*<7PhNtG}i%LSl&|x@qj@eFGJX8DFlgb9neEf zV9u!Ks?c@Af)x8_M_&ePr7{ZUvhpHGV0kK7iZ2o1Wrg;zooD}Qd^wGz1^rY2ov0Gv zHC@Mvp#e^yE2x!;m`KDYeg;xXN`W(+ddrIdZ;-_M+aN3c)0Vf7%U3ZYAXt&mqv01cd_&X`@`i82Kmx zVrU!J9_1MR!_6@`@1U}hs4y1ps|!2)as(_gQr)!~V|fQx^io;@Ei{^2N#Hl9M>ei+ z8Z%Q4XdsB6lwo@cd+j3PP z>=V2$`w+C|TdKaQIUthP>T}`lsv{?$X@IpDL8y|&egW46sz7)- zKtpZ8U_7KzQ`ShC(T=+%9(QRj{_XsLsp;<+Yj?s&8Ny&WU6KnoI@Vg{a|FCYQm|2=4kvJGbg#u2Ne&YQfy;q=1#@T>us|W z=Vvt!IERHoN((!{3OQ1(k!b9|OdPi3M?RouyY{DlSil9sF#~zD-O)V$@)%5RhGa%no_CU_lBENr19v9|!9I`19-wG@kM7Isg8ve6v5#{?D^- z*B(KOeuQWL|6IAwh=-Msy!*&P$l$^PMv;;s&AH|!g#nN6@;-ZAk@kbE8oa=>et6Kz zsH-K%lU1ez{@c1n7_kgBuCcIBcD?@=5$ZIeVb+Afq(=`Lg)A%^_25*3P zz=i2r+yVgJo47VRxy*?ih84k@We(cCPa(OrwHm2|;>_0+xTZXNYu4l{_~mnBR#6Ig z0P}!-SJy*wRG2}I^ka5vG3yLJ5qhrIEH{e?cznc8!iupLyH){F*TgsL9wJ#V8L(?S zUW)+N9AIPEJ%ussd*+Z0Tf!}23*105G=TR&P)x2cCH-#S5>-$$hFVksW(@zL3Wcsi z9z7k*i;94PI=ml#G{SY1f1-|)X3Jf64yyuqFVqTwt}#1{d!6C0UXH0}9iy#a;JGh` zY5TcvOog516YgHqMtmh7AV77|eA>^5cQ`GLX4gXA$J@y6taAoNKMCA=w)y&!=-wVF zy^ppj;UOS3;1ZiP;67`Wf^ z73Whdw^9mD$6F}FQU^(nUSmnhb0)6@1b>Vsdk$9;JX}T?VFdGvzx|2!>Fg98EpQbo zX}QJS3^!=F8>+}A0Goh0(Wo0pz>H1N*tKvD{^h*HB@wUJv|nf`hU;(T=<^T=UyeKa zQ>W9R_5Mw$0M<6*P`cbEmsO2_84o994&Yv;unp(C&NW=1(Q9~gPGW{&+oGSsU<8SU z6{Z#3wYP!@x2$WU96Q`uU{63Gfp73vAbi4g$hoY^RRlKzL4>PxeX?CV`M_+iczYIb zRsXr-;!C_UNG>j>`H!?N^*=v zcm-(X(l8GnT(~+{XlN(z^a_nR{<$f5HRCtgSl44rniJm?03VFs#{$k2s*=X|{ayJK zKz_C|fIYW+Dv=V6OpLT6jc@l;iRTD3a>>umv1VC93LuS#%NU71DuA&6Gn0!?fYyc; z!zDF_^B+cms~fn;%>J5K5q9H)zv(w}I?qxKk{Qq{9$^TR8D8o!vNrtKM)KQ~crjyi z!GUjz-9m5&EAhx*p%BOhX!IYCoh!TYhwW$-&%^KYOX7it_d=n#uNH?&g@P{9*}SC@ z5L}VSEM4So#lt!;j$vT3A63Eq$@o}65^44$21j$FV_n)*bj;+IuE#V3TIGl)ml9IJ za$Jw6;((gV)z7!BiyS+|NSQY0m5WWQ!gPxP7-nVAd^21{0tw3qM3c)r)_lWrJTHk! zj~v50i{*URw7f|uf1*RPv~bN7t|byg8aa&TxX!CIs?@B@UMkCpp2isRtqSI%tKh{i z0aCp+I3ER!6zAgtzz0>_^B76>gE49U;j9!R6>-i)Ze;fdmo@rA7aj1xEF5b>fT7Y% zJYGb*E&_Utnr9Ell1?GDY*WT5TqSV2<+U}bpar}yCc49vz^B;K2r=SG{^N|0ctrNXidp+l z5253X3XLl+rVRteLqJRTWd?ZXeo7H1@dfwnsgY>VzQXT`H-QL@7o0c{b+s}BF=`U3 zx*O-fWjN()#TX}cAP^wNv(Sh!R$j!onWQ}+9#*80wgnK~uxJj(n_%uF>av8lIG#~1 z`T}EZOka^AyA;yM_Sa14BOY~2^>!};xZy|%*vLb`!q&bfK}(t0nAAPX48u#AnT^4C z5i>nXInsKq*(k&QI!uLpt~}hDOeYtE%_(@uKf0GL2j0zx28><9NqCLL{WsPuTHP5A zVFiu7teN5x`hdotHT&OKvu?qHQ1=*1AA)|xIrHe_496>p-SnOq%WDhcxOfk$50~m) zYfcxSpgrDpV8BG>U5SwpFJ%!U8XnLf@b9U{ksm;XC!srcu(^r2`ZPzk)KI^gm{KVB z`a>+K;&f6L!eJ1WINIi#yh5HO$??7a0YL5mdumNbcS_|ZV&a7YTIY8qCqKGom>*vgqiI~ zPQSdf6dpd>g_$0e`#uH-O87y5J?dcJ9g^JsOmnI0CJp~H&FwVyH_bi>&vE@{nwd67 zu78nfX1AnF(dOq<8m29PCt0?)z8tR-P_y6|a8XS?%$%u!OAX+6jEtIzzgu|8PAqE7 z@C8+z!0R!p>Aj_Qs#1%Gn;TuXqzhBNMSILy0Ik~10(ktX^AL{O09F=#?No+WfyOMQ z$#cKpB#HkvC#DYK;TBiU@e=EC!JVjIRM$)!9T{UI%dmWA; z*WJXkq)IDLgEgNJj}uD)(4Vwew=L`fygb8R0AloivveJ>5Nv_4PYQh4i!R5QA08*4 zzYmXj0wWW(R>}(bS!kcGVsZvf<5@eqhfM0ptP}$Y_t*EZuJRYa-l?A=asA`6QhF@K zMkBKkXN_O621y6BVH9b!T?qtY0QBOFjT%y$mBN4kSF$`2NRudq5xLj6!47PsV(pH`I zdyKaJ+4*`SHC-UAu31>V<+LZpE_+bi;`I6g#VsE;<U2oX^a~#&{#muTW8wx Y9}5B^N};j!qW}N^07*qoM6N<$g4vl8cK`qY 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..3e57d9d946 --- /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:618571.8 %
Date:2024-01-07 21:47:14Functions: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)1345
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)1345
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const1345
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const1986
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const2690
mrs_uav_managers::Estimator::getMaxFlightZ() const9439
mrs_uav_managers::Estimator::isStarted() const28177
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)176926
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)177001
mrs_uav_managers::Estimator::isReady() const206632
mrs_uav_managers::Estimator::getName[abi:cxx11]() const360662
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const406883
mrs_uav_managers::Estimator::isMitigatingJump() const430975
mrs_uav_managers::Estimator::publishDiagnostics() const581518
mrs_uav_managers::Estimator::isError() const697325
mrs_uav_managers::Estimator::isRunning() const995150
mrs_uav_managers::Estimator::isInitialized() const1504501
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const4462479
mrs_uav_managers::Estimator::getCurrentSmState() const4908233
+
+
+ + + +
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..d2edbd3a7c --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:618571.8 %
Date:2024-01-07 21:47:14Functions: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)1345
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)176926
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)177001
mrs_uav_managers::Estimator::getHeadingRate(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)1345
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const406883
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const1986
mrs_uav_managers::Estimator::getMaxFlightZ() const9439
mrs_uav_managers::Estimator::isInitialized() const1504501
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const2690
mrs_uav_managers::Estimator::isMitigatingJump() const430975
mrs_uav_managers::Estimator::getCurrentSmState() const4908233
mrs_uav_managers::Estimator::publishDiagnostics() const581518
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const1345
mrs_uav_managers::Estimator::getName[abi:cxx11]() const360662
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const697325
mrs_uav_managers::Estimator::isReady() const206632
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const4462479
mrs_uav_managers::Estimator::isRunning() const995150
mrs_uav_managers::Estimator::isStarted() const28177
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..10c389dc1c --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html @@ -0,0 +1,290 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:618571.8 %
Date:2024-01-07 21:47:14Functions: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        1345 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10        1345 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14        1345 :   previous_sm_state_ = getCurrentSmState();
+      15        1345 :   setCurrentSmState(new_state);
+      16             : 
+      17        1345 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18        1345 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23     4462479 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24     4462479 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29     1504501 : bool Estimator::isInitialized() const {
+      30     1504501 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35      206632 : bool Estimator::isReady() const {
+      36      206632 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41       28177 : bool Estimator::isStarted() const {
+      42       28177 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47      995150 : bool Estimator::isRunning() const {
+      48      995150 :   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      697325 : bool Estimator::isError() const {
+      60      697325 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65     4908233 : SMStates_t Estimator::getCurrentSmState() const {
+      66     4908233 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71        1345 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72        1345 :   std::scoped_lock lock(mutex_current_state_);
+      73        1345 :   current_sm_state_ = new_state;
+      74        1345 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78        2690 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79        2690 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84        1345 : std::string Estimator::getCurrentSmStateString(void) const {
+      85        2690 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90      430975 : bool Estimator::isMitigatingJump(void) const {
+      91      430975 :   return is_mitigating_jump_;
+      92             : }
+      93             : /*//}*/
+      94             : 
+      95             : /*//{ getName() */
+      96      360662 : std::string Estimator::getName(void) const {
+      97      360662 :   return name_;
+      98             : }
+      99             : /*//}*/
+     100             : 
+     101             : /*//{ getPrintName() */
+     102        1986 : std::string Estimator::getPrintName(void) const {
+     103        3973 :   return ch_->nodelet_name + "/" + name_;
+     104             : }
+     105             : /*//}*/
+     106             : 
+     107             : /*//{ getType() */
+     108           0 : std::string Estimator::getType(void) const {
+     109           0 :   return type_;
+     110             : }
+     111             : /*//}*/
+     112             : 
+     113             : /*//{ getFrameId() */
+     114      406883 : std::string Estimator::getFrameId(void) const {
+     115      406883 :   return ns_frame_id_;
+     116             : }
+     117             : /*//}*/
+     118             : 
+     119             : /*//{ getMaxFlightZ() */
+     120        9439 : double Estimator::getMaxFlightZ(void) const {
+     121        9439 :   return max_flight_z_;
+     122             : }
+     123             : /*//}*/
+     124             : 
+     125             : /*//{ publishDiagnostics() */
+     126      581518 : void Estimator::publishDiagnostics() const {
+     127             : 
+     128      581518 :   if (!ch_->debug_topics.diag) {
+     129      581493 :     return;
+     130             :   }
+     131             : 
+     132           0 :   mrs_msgs::EstimatorDiagnostics msg;
+     133           0 :   msg.header.stamp       = ros::Time::now();
+     134           0 :   msg.header.frame_id    = getFrameId();
+     135           0 :   msg.estimator_name     = getName();
+     136           0 :   msg.estimator_type     = getType();
+     137           0 :   msg.estimator_sm_state = getCurrentSmStateString();
+     138             : 
+     139           0 :   ph_diagnostics_.publish(msg);
+     140             : }
+     141             : /*//}*/
+     142             : 
+     143             : /*//{ getAccGlobal() */
+     144           0 : tf2::Vector3 Estimator::getAccGlobal(const sensor_msgs::Imu::ConstPtr& input_msg, const double hdg) {
+     145             : 
+     146           0 :   geometry_msgs::Vector3Stamped acc_stamped;
+     147           0 :   acc_stamped.vector = input_msg->linear_acceleration;
+     148           0 :   acc_stamped.header = input_msg->header;
+     149           0 :   return getAccGlobal(acc_stamped, hdg);
+     150             : }
+     151             : 
+     152      177001 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     153             : 
+     154      352173 :   geometry_msgs::Vector3Stamped acc_stamped;
+     155      176685 :   acc_stamped.vector = input_msg->control_acceleration;
+     156      176640 :   acc_stamped.header = input_msg->header;
+     157      350828 :   return getAccGlobal(acc_stamped, hdg);
+     158             : }
+     159             : 
+     160      176926 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     161             : 
+     162             :   // untilt the desired acceleration vector
+     163      352178 :   geometry_msgs::Vector3Stamped des_acc;
+     164      176540 :   geometry_msgs::Vector3        des_acc_untilted;
+     165      176639 :   des_acc.vector.x        = acc_stamped.vector.x;
+     166      176639 :   des_acc.vector.y        = acc_stamped.vector.y;
+     167      176639 :   des_acc.vector.z        = acc_stamped.vector.z;
+     168      176639 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     169      176768 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     170      353470 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     171      177022 :   if (response_acc) {
+     172      177022 :     des_acc_untilted.x = response_acc.value().vector.x;
+     173      177022 :     des_acc_untilted.y = response_acc.value().vector.y;
+     174      177022 :     des_acc_untilted.z = response_acc.value().vector.z;
+     175             :   } else {
+     176           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform from %s to %s failed", getPrintName().c_str(), des_acc.header.frame_id.c_str(),
+     177             :                       ch_->frames.ns_fcu_untilted.c_str());
+     178             :   }
+     179             : 
+     180             :   // rotate the desired acceleration vector to global frame
+     181      177022 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     182             : 
+     183      350643 :   return des_acc_global;
+     184             : }
+     185             : /*//}*/
+     186             : 
+     187             : /*//{ getHeadingRate() */
+     188           0 : std::optional<double> Estimator::getHeadingRate(const nav_msgs::OdometryConstPtr& odom_msg) {
+     189             : 
+     190             :   double hdg_rate;
+     191             :   try {
+     192           0 :     hdg_rate = mrs_lib::AttitudeConverter(odom_msg->pose.pose.orientation).getHeadingRate(odom_msg->twist.twist.angular);
+     193             :   }
+     194           0 :   catch (...) {
+     195           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading rate", getPrintName().c_str());
+     196           0 :     return {};
+     197             :   }
+     198           0 :   return hdg_rate;
+     199             : }
+     200             : /*//}*/
+     201             : 
+     202             : 
+     203             : /*//}*/
+     204             : 
+     205             : }  // namespace mrs_uav_managers
+     206             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html new file mode 100644 index 0000000000..109fe8a5e8 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html @@ -0,0 +1,72 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimator.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..0542f334157864cbe9b6e41d42d270c218eb0f56 GIT binary patch literal 799 zcmV+)1K|9LP)xi7#@*z z89sxuW_h=)(VdW4bFnr6Yu42TYmzd?nk4gn))ynZL%a3$-(*d2{|MGc*@|M^hbT}} zsBl@R$|bCD`hQ=)mNoD1gFzL!82gfShuMd)ZZ<2Z+1pyMMoiR_NkFAu6cl|k1K=(5 zbeiiB6l$v^TWAd?`navo4GWz02Ka_G63M>dI0|dV*VqYZZP!?`rl}3UT9WK*U!$ZG zYnIahtT|8P5zZ%jcZOUE6AZwb_jPPzvbdgQr`s8}Ss9?6VO@>+8trPJulp{1zpo`n zQb{vYvhMO4gEiytHiuK$OiE@cH6pr_M$Uo}C^V$pJldAB6FWBp-C*CrfCM^C5!lyg zqegCOInWU)&QzuX3Z*s=nX`t-W?VW;dAmyE_2|BN1VOP^^IIN>*|t446Su^2&xj>u zhbmdr>Err^Z%(&(CBZMHkqnFi(dMJ+!-@N3c@BT69la^!cAWfskNMaSvBKlq4Kp~Z z5d!@K%9a`EN1$c+P2iSJ&ZxT~u+UgzJ#bCW(bL@ddoFS0O;6NFN3Qy5pz73K2-Lbv zCnn^y-S|(=H!wfiMhr|TKk#}Fi}3TSi1a0;k>nxFjp|yGiiYk2$LRL@#jq(_#8_Sn zr)PNU$}e)3u|A`#DVUAOUw-QDRWt;`z&3e1m^_T;IYxr1FRbxY&jH`qa*5^jQ_qiZ dVh{3dg + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-01-07 21:47:14Functions: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&) const47
mrs_uav_managers::AglEstimator::publishAglHeight() const79755
mrs_uav_managers::AglEstimator::publishCovariance() const79755
+
+
+ + + +
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..efeadca982 --- /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-01-07 21:47:14Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const79755
mrs_uav_managers::AglEstimator::publishCovariance() const79755
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const47
+
+
+ + + +
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..70b751b113 --- /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-01-07 21:47:14Functions: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       79755 : void AglEstimator::publishAglHeight() const {
+       8       79755 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9       79755 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13       79755 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15       79755 :   if (!ch_->debug_topics.covariance) {
+      16       79755 :     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          47 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26          47 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27          47 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29          47 :   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          47 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35          47 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36          47 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37          47 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38          47 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39          47 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40          47 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41          47 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42          47 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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          47 :   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..fff89c5ebd --- /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-01-07 21:47:14Functions: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..e629a30682 --- /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-01-07 21:47:14Functions: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..80f2c1df94 --- /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-01-07 21:47:14Functions: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..3a8338c729 --- /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-01-07 21:47:14Functions: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..1c4352427b --- /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-01-07 21:47:14Functions: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..9edd67573b --- /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-01-07 21:47:14Functions: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..b5a0aa655c --- /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-01-07 21:47:14Functions: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&) const61
mrs_uav_managers::StateEstimator::getInnovation() const94481
mrs_uav_managers::StateEstimator::getPoseCovariance() const94481
mrs_uav_managers::StateEstimator::getTwistCovariance() const94481
mrs_uav_managers::StateEstimator::getUavState()103693
mrs_uav_managers::StateEstimator::publishUavState() const113294
mrs_uav_managers::StateEstimator::publishOdom() const113304
mrs_uav_managers::StateEstimator::publishCovariance() const113304
mrs_uav_managers::StateEstimator::publishInnovation() const113304
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const230008
+
+
+ + + +
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..ffb81e8267 --- /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-01-07 21:47:14Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::getUavState()103693
mrs_uav_managers::StateEstimator::publishOdom() const113304
mrs_uav_managers::StateEstimator::getInnovation() const94481
mrs_uav_managers::StateEstimator::publishUavState() const113294
mrs_uav_managers::StateEstimator::getPoseCovariance() const94481
mrs_uav_managers::StateEstimator::publishCovariance() const113304
mrs_uav_managers::StateEstimator::publishInnovation() const113304
mrs_uav_managers::StateEstimator::getTwistCovariance() const94481
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const61
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const230008
+
+
+ + + +
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..2db2d90e13 --- /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-01-07 21:47:14Functions: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      103693 : std::optional<mrs_msgs::UavState> StateEstimator::getUavState() {
+       9             : 
+      10      103693 :   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      207372 :   return mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      16             : }
+      17             : /*//}*/
+      18             : 
+      19             : /*//{ getInnovation() */
+      20       94481 : nav_msgs::Odometry StateEstimator::getInnovation() const {
+      21       94481 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      22             : }
+      23             : /*//}*/
+      24             : 
+      25             : /*//{ getPoseCovariance() */
+      26       94481 : std::vector<double> StateEstimator::getPoseCovariance() const {
+      27       94481 :   return mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_.values);
+      28             : }
+      29             : /*//}*/
+      30             : 
+      31             : /*//{ getTwistCovariance() */
+      32       94481 : std::vector<double> StateEstimator::getTwistCovariance() const {
+      33       94481 :   return mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_.values);
+      34             : }
+      35             : /*//}*/
+      36             : 
+      37             : /*//{ publishUavState() */
+      38      113294 : void StateEstimator::publishUavState() const {
+      39             : 
+      40      113294 :   if (!ch_->debug_topics.state) {
+      41           0 :     return;
+      42             :   }
+      43             : 
+      44      226285 :   auto uav_state = mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      45      113127 :   ph_uav_state_.publish(uav_state);
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ publishOdom() */
+      50      113304 : void StateEstimator::publishOdom() const {
+      51             : 
+      52      226608 :   auto odom = mrs_lib::get_mutexed(mtx_odom_, odom_);
+      53      113304 :   ph_odom_.publish(odom);
+      54      113304 : }
+      55             : /*//}*/
+      56             : 
+      57             : /*//{ publishCovariance() */
+      58      113304 : void StateEstimator::publishCovariance() const {
+      59             : 
+      60      113304 :   if (!ch_->debug_topics.covariance) {
+      61      113304 :     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      113304 : void StateEstimator::publishInnovation() const {
+      73             : 
+      74      113304 :   if (!ch_->debug_topics.innovation) {
+      75      113304 :     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      230008 : std::optional<geometry_msgs::Quaternion> StateEstimator::rotateQuaternionByHeading(const geometry_msgs::Quaternion& q, const double hdg) const {
+      85             : 
+      86             :   try {
+      87      230008 :     tf2::Quaternion tf2_q = mrs_lib::AttitudeConverter(q);
+      88             : 
+      89             :     // Obtain heading from quaternion
+      90      229760 :     double q_hdg = 0;
+      91      229760 :     q_hdg        = mrs_lib::AttitudeConverter(q).getHeading();
+      92             : 
+      93             :     // Build rotation matrix from difference between new heading and quaternion heading
+      94      229474 :     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      229337 :     geometry_msgs::Quaternion q_new = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_q);
+      98      228910 :     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          61 : bool StateEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+     109             : 
+     110          61 :   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          61 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+     116          61 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+     117          61 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+     118          61 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+     119          61 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+     120          61 :   ph_->param_loader->loadParam("requires/position", requires_position);
+     121          61 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+     122          61 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+     123          61 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+     124             : 
+     125          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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..b141afb626 --- /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:42661369.5 %
Date:2024-01-07 21:47:14Functions: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.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
estimation_manager.cpp +
69.1%69.1%
+ 		69.1 %365 / 52887.5 %21 / 24
<unnamed>69.1 %365 / 52887.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..444506cd62 --- /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:42661369.5 %
Date:2024-01-07 21:47:14Functions: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.1%69.1%
+ 		69.1 %365 / 52887.5 %21 / 24
<unnamed>69.1 %365 / 52887.5 %21 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail.html b/mrs_uav_managers/src/estimation_manager/index-detail.html new file mode 100644 index 0000000000..57230bac4f --- /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:42661369.5 %
Date:2024-01-07 21:47:14Functions: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.1%69.1%
+ 		69.1 %365 / 52887.5 %21 / 24
<unnamed>69.1 %365 / 52887.5 %21 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
<unnamed>71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-f.html b/mrs_uav_managers/src/estimation_manager/index-sort-f.html new file mode 100644 index 0000000000..24eea1e07f --- /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:42661369.5 %
Date:2024-01-07 21:47:14Functions: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.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
estimation_manager.cpp +
69.1%69.1%
+ 		69.1 %365 / 52887.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..43c55d3a10 --- /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:42661369.5 %
Date:2024-01-07 21:47:14Functions: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.1%69.1%
+ 		69.1 %365 / 52887.5 %21 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index.html b/mrs_uav_managers/src/estimation_manager/index.html new file mode 100644 index 0000000000..75653eb127 --- /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:42661369.5 %
Date:2024-01-07 21:47:14Functions: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.1%69.1%
+ 		69.1 %365 / 52887.5 %21 / 24
estimator.cpp +
71.8%71.8%
+ 		71.8 %61 / 8582.6 %19 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..e7cd0f49f0 --- /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:20325380.2 %
Date:2024-01-07 21:47:14Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::gain_manager::GainManager::onInit()57
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)57
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)1199
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&)1258
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)12206
+
+
+ + + +
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..2377b7bfaa --- /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:20325380.2 %
Date:2024-01-07 21:47:14Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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&)1258
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)1199
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)12206
mrs_uav_managers::gain_manager::GainManager::onInit()57
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)57
+
+
+ + + +
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..3e7c70508c --- /dev/null +++ b/mrs_uav_managers/src/gain_manager.cpp.gcov.html @@ -0,0 +1,720 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/gain_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - gain_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:20325380.2 %
Date:2024-01-07 21:47:14Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/gain_manager.cpp.gcov.png b/mrs_uav_managers/src/gain_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ea07e2cfccd5918ef5761030b44db95ce6a47f1b GIT binary patch literal 2099 zcmV-32+a41P)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpAPlxQa8?K#;1!Y$oc&jMF{c0t1jtR>SMoK7`$yplcf!h(U3noG=)3MAvK}NizZRd91)^6kcP1(FhZk-Lp-I z04wHU@i_T?gyP8*M}y)$!>;SV3%XuU!&uv_BIKPcZJ@j`i*>Wra$w(8fSY;ZQ1s?$ z0au3Lo-;Y|Z;nARikmcrgA~N-mbw2sb<5s)IQV0H5VjIA}R4g zH5DFHigOGunR;sS=IWV`9Kdn6_#8|nJqlTAa!GMX6ds7=NmFLRDvyw2w~PqIB${n0 zBU5^0;3yy=%Q=O^;HvO_E0|HBFdL5pmqqzq)zc%AF0hcA zDa_sbihC{sPoZaT?dblAJ(os1N+t%LZZSmBa@7Kk z@@NdLiYfU6KrIgR&>o7P2rMBwBiuK&`c()g5$NOluX0@T^_pBf$JL#4VG#?n4lI!A zWiJSP9v4CB=gs$`B1RpH$kO>44KcKE?dh54Cz{ofKIeH9|32R6N1jB0ifp9g7NP zGq*Oa?IlG=BaRDHV$)a{)K6uW0w^KaL$#)=rzCZka`_VaE|D2-k6sQS{ zCSk%No#9hVn)lqp(LW-vkK*qzp_w&ZwRLsMeWc^RCJd9uXzN!rthMqu#a}dBxjf3} z;nh(HIR9y*u$9h8kHX))Rw!<$y!-^xwpHY(Rge=%aB>(I=|i_bRTucKe4az`x|+DS z&_=g6VA+o6l>jmuQYL{Pm3~+~u)sI3@_{@uyY4FvVT6@yj(k%lb4!ohOX>@@soEm# z-3*lTQ`Jhkw+3kqk>VD9o5BAulex5QsAU`J8P05MS=yZyABhg z+ynA>h#FXZ*cj$8^g_D zrvA*-^4#;fZ>;jW(=xy<$dX0fQ#`lWsW`3}=v9NTA`@Y<;bn)IHjh-8nuqswoy-dr zg|jWgG0iB&SujvhQW5xCie>1j8b;b7>sAkv<4-I6D22U9HG& zSFROgvT!^PS3oa5N&L&hKaz(hF1qh7$f|fr@1NDVD2vz;r=W@unGN}o2)yF<5}-Vf zqZD^M1fg{)&;mcZ>p<=cfbu*JQq;uFnux;js=%-L^(VBCAGTKLJ6E{nvGgm>6~u0C zBJXT+^@rehXP73$e+~fL^ycR7Q0mrlj^%Xx%6&=z_+J*!Z7VEAlcy*_)9+-%h62lo dk_Q<3@(%^1?apzBlg1 literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/index-detail-sort-f.html b/mrs_uav_managers/src/index-detail-sort-f.html new file mode 100644 index 0000000000..779dc91de8 --- /dev/null +++ b/mrs_uav_managers/src/index-detail-sort-f.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/srcHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1063169062.9 %
Date:2024-01-07 21:47:14Functions:577378.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
constraint_manager.cpp +
65.6%65.6%
+ 		65.6 %149 / 22775.0 %6 / 8
<unnamed>65.6 %149 / 22775.0 %6 / 8
gain_manager.cpp +
80.2%80.2%
+ 		80.2 %203 / 25385.7 %6 / 7
<unnamed>80.2 %203 / 25385.7 %6 / 7
uav_manager.cpp +
58.2%58.2%
+ 		58.2 %670 / 115189.5 %34 / 38
<unnamed>58.2 %670 / 115189.5 %34 / 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..071dec3bdf --- /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:1063169062.9 %
Date:2024-01-07 21:47:14Functions:577378.1 %
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 +
58.2%58.2%
+ 		58.2 %670 / 115189.5 %34 / 38
<unnamed>58.2 %670 / 115189.5 %34 / 38
constraint_manager.cpp +
65.6%65.6%
+ 		65.6 %149 / 22775.0 %6 / 8
<unnamed>65.6 %149 / 22775.0 %6 / 8
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
80.2%80.2%
+ 		80.2 %203 / 25385.7 %6 / 7
<unnamed>80.2 %203 / 25385.7 %6 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index-detail.html b/mrs_uav_managers/src/index-detail.html new file mode 100644 index 0000000000..3fbbdca79b --- /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:1063169062.9 %
Date:2024-01-07 21:47:14Functions:577378.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
65.6%65.6%
+ 		65.6 %149 / 22775.0 %6 / 8
<unnamed>65.6 %149 / 22775.0 %6 / 8
gain_manager.cpp +
80.2%80.2%
+ 		80.2 %203 / 25385.7 %6 / 7
<unnamed>80.2 %203 / 25385.7 %6 / 7
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
<unnamed>69.5 %41 / 5955.0 %11 / 20
uav_manager.cpp +
58.2%58.2%
+ 		58.2 %670 / 115189.5 %34 / 38
<unnamed>58.2 %670 / 115189.5 %34 / 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..a3725ea5ce --- /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:1063169062.9 %
Date:2024-01-07 21:47:14Functions:577378.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
constraint_manager.cpp +
65.6%65.6%
+ 		65.6 %149 / 22775.0 %6 / 8
gain_manager.cpp +
80.2%80.2%
+ 		80.2 %203 / 25385.7 %6 / 7
uav_manager.cpp +
58.2%58.2%
+ 		58.2 %670 / 115189.5 %34 / 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..965a45ac6c --- /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:1063169062.9 %
Date:2024-01-07 21:47:14Functions:577378.1 %
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 +
58.2%58.2%
+ 		58.2 %670 / 115189.5 %34 / 38
constraint_manager.cpp +
65.6%65.6%
+ 		65.6 %149 / 22775.0 %6 / 8
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
80.2%80.2%
+ 		80.2 %203 / 25385.7 %6 / 7
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/index.html b/mrs_uav_managers/src/index.html new file mode 100644 index 0000000000..9f9238c577 --- /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:1063169062.9 %
Date:2024-01-07 21:47:14Functions:577378.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
constraint_manager.cpp +
65.6%65.6%
+ 		65.6 %149 / 22775.0 %6 / 8
gain_manager.cpp +
80.2%80.2%
+ 		80.2 %203 / 25385.7 %6 / 7
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
uav_manager.cpp +
58.2%58.2%
+ 		58.2 %670 / 115189.5 %34 / 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..e6b33bb887 --- /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-01-07 21:47:14Functions: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&)3
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_managers::NullTracker::~NullTracker()57
mrs_uav_managers::NullTracker::~NullTracker().257
mrs_uav_managers::NullTracker::deactivate()131
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)134
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)182
mrs_uav_managers::NullTracker::getStatus()2998
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)64638
+
+
+ + + +
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..3f291bbe36 --- /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-01-07 21:47:14Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::NullTracker::deactivate()131
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>)57
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&)182
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
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&)3
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&)64638
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)134
mrs_uav_managers::NullTracker::getStatus()2998
mrs_uav_managers::NullTracker::~NullTracker()57
mrs_uav_managers::NullTracker::~NullTracker().257
+
+
+ + + +
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..9d8410d520 --- /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-01-07 21:47:14Functions: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         114 :   ~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          57 : 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         114 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60          57 :   ros::Time::waitForValid();
+      61             : 
+      62          57 :   is_initialized = true;
+      63             : 
+      64          57 :   this->common_handlers = common_handlers;
+      65             : 
+      66          57 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68         114 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75         134 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77         134 :   std::stringstream ss;
+      78         134 :   ss << "activated";
+      79             : 
+      80         134 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81         134 :   is_active = true;
+      82             : 
+      83         268 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90         131 : void NullTracker::deactivate(void) {
+      91             : 
+      92         131 :   ROS_INFO("[NullTracker]: deactivated");
+      93         131 :   is_active = false;
+      94         131 : }
+      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       64638 : 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       64638 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126        2998 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128        2998 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130        2998 :   tracker_status.active            = is_active;
+     131        2998 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133        2998 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140         169 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142         338 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144         169 :   std::stringstream ss;
+     145             : 
+     146         169 :   if (cmd->data != callbacks_enabled) {
+     147             : 
+     148           7 :     callbacks_enabled = cmd->data;
+     149             : 
+     150           7 :     ss << "callbacks " << (callbacks_enabled ? "enabled" : "disabled");
+     151             : 
+     152           7 :     ROS_DEBUG_STREAM("[NullTracker]: " << ss.str());
+     153             : 
+     154             :   } else {
+     155             : 
+     156         162 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159         169 :   res.message = ss.str();
+     160         169 :   res.success = true;
+     161             : 
+     162         507 :   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           3 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr NullTracker::setTrajectoryReference([
+     187             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     188           3 :   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         182 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240         182 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248          57 : 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:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
<unnamed>80.7 %352 / 43692.3 %12 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/src/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..0ef136a32a --- /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:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
<unnamed>80.7 %352 / 43692.3 %12 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-detail.html b/mrs_uav_managers/src/transform_manager/index-detail.html new file mode 100644 index 0000000000..0c2fe1c970 --- /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:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
<unnamed>80.7 %352 / 43692.3 %12 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-sort-f.html b/mrs_uav_managers/src/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..eabab5c37a --- /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:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index-sort-l.html b/mrs_uav_managers/src/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..c8eedb6969 --- /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:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/index.html b/mrs_uav_managers/src/transform_manager/index.html new file mode 100644 index 0000000000..08d0d9f72a --- /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:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
transform_manager.cpp +
80.7%80.7%
+ 		80.7 %352 / 43692.3 %12 / 13
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..8ec8c9c657 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::transform_manager::TransformManager::getName[abi:cxx11]() const0
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::transform_manager::TransformManager::onInit()57
mrs_uav_managers::transform_manager::TransformManager::TransformManager()57
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const867
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)4281
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)53853
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)84236
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)94453
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)106370
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)113015
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)113015
+
+
+ + + +
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..a691ea3763 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)53853
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)4281
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)94453
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)84236
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)106370
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)113015
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)113015
mrs_uav_managers::transform_manager::TransformManager::onInit()57
mrs_uav_managers::transform_manager::TransformManager::TransformManager()57
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const867
mrs_uav_managers::transform_manager::TransformManager::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5
mrs_uav_managers::transform_manager::TransformManager::getName[abi:cxx11]() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e54ea334d2 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html new file mode 100644 index 0000000000..80886f60c3 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html @@ -0,0 +1,1030 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/transform_manager - transform_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35243680.7 %
Date:2024-01-07 21:47:14Functions:121392.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9e1ba3a4ac70a3c69b1f999ba451d2b9a6a46d46 GIT binary patch literal 3258 zcmV;r3`O&aP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp3;FbC}F`p{s`lT4!cGT;WrPVSd>t&zW? z>l^5MlskZ^5xzOt_x+Yh9n}fDQcO!~^Q5(5nil64rMV2WwT)u>Y9f3T=*(RkfkRxa z7Kx8gDGtnCp_mrK?U*t!6oVBy%%xpRQP3EKGxM7T>4@MK=?{~9MlM;pwKSVt3ZtXrr;8Bq$D(5&%K#^&CI}yl@ z`z0@sIxvrDTE;Zo?bVoGcM=z6NJ%M?6XXXgE(0W=h`<~Cq}g@nwn>=ldgmfb6MgB7 zuJg>^8eN+*EL~m0k%uWXkvXC38y$n1TPkuY@7cV~J#POxI23r~e7Brf0vsL_&&a;+ zqKMT*7~5c`$%*=xT4Y}d{$qQE%P97h0?&?USPHKl1==aXrU82?CPrt0&2EY_lIF3j zLi(maGkr?LBa3O5z-Rj6ka(~}7>VHStt56KT|SqQ}`(5432SUCn}cMzEh~d*r6#PQ?T`kzqfWIS<;2ZnFbu zY|>HyBJZcj`oPTuA`3G-9zUV2;otV-x%wYJ&!5r?(EpxqYbVgwUe?T8Mpi^FOluO4 zmzvY|JOvm8;{UlsYF^OtfEHKaAbfuh&;kFOKNENi&dY@fgi|{&9k%CYJgO>F_yjWs zXLF@<-I&71YMkqybsePRD$&uU&<|{Bk5S-g6)LA}Sm4jse{lt_@v}9q%_{_ce$ZvL z;S<(iO1g6}c72ETsArj#z`@I|!636|9<>InZBJsR&7(s=GKuwIgxPi#P^M^Ait~4S z4LRo51SlBrm*@_M%~ex)ESZ5oaevohcE+XmL=kB^j1AcfrMUo0U>HKyIloEo-4gr^ zVNM#12Uhzq{0Cvo`%~IXU{u$xC zk$TxDN6#pTmFg?0#@T&f{ag*)UdLZ1peqh20W-mnI8L4hd8$4hp_dd+z56Jdjv%2g zWO_tEtBbR?$#?{*^N2IU7xJhcLFznW-~~MDtrQG2fRhy%p?fYiNGV0m8S z7K^RAs~K~}^}GL6N?Qsxig-J-^5;$KuWx5o4E)UP4Bona;&$d2wuNd-{hhY&$-bc9%G`2)hyXfbC)t@4fEBwN*cO+f(>xexk;;;l()7#0gS8!cj17A2hxcSbn z<_MfxV|H=WG{qry?J4T>GA~`blnc^|Ol1;xh9n)Gr+vZmr>aY7S=>u&6*Q63GFvp| z2xy#`X;^Hz{f><$Ec8BFoE6%_Wj^WI-0W*-jH8Ajmnv|t6o&b}P$_cUE-dgKYc`gv z?0VPus6&=xIgASp8VwdW*??tI0#_nw&2hrP zKnH}~kYjE0d7!I+DE(lTW7y}7TQW?0k_QYARz<7YG4_Xd>GlgjiR}gIy3Ig_Iq;RY0Hh%CWsq30p4k)Q9aKr zQQb`?ZAGlO=h(G>l?p4LK68QtaT;9Hu8nI3hOo|m$h;VnHUi!xo;nJRXj(v zE}spbebC*(W0V`UKIMZfM;niSD*a&baM8t?JRAlp9L58`J;59fgo9#q)u@L9Cud}CDnt54 z_c7$hM}_S47@iY8<_AiRv7FcF6u441Y=qF_qCn0F?XZu?ABFkYdB>7rVx+ z^Ayc(;!CAxrZn9+m?4Ck{<#KWUPC2hPyk;K8O|mqw%VOuoSu75d?s=ZAf5tDWVy)a z3ieSCam={_>z1mqyt?0#z-_~3f(o9=3~cha{m**G$7%xJ^{A#{6L&~uBtw#{N=R~~ z*Hu>G__LaP-${)_XR&u;UMJ(~l9ZzI+ft-i$1@O!vR!l3HgB?`kvdx`j7=CyH7pVW za6?Je59i-ax$Lf#a5!3cG#~F@ba_9OhMw{$P5yYl{_uOe-x@VpAWOZ|BS1Aa!xU+4 zuLN|O^6npu;DrwX`C-$mwv&OBr_1LRsMwvzh$LOU?n8ATl2@R9;p-L5qgF>w zk2DmD)-v0VP_xDkg;TRi=GT;_dx4wIXRfSkR!d3V%j4NgVYplWG?p3IKNzTBIpYNd zZX66=*UGcGnVN=hWhVWM?)1H8?@tW|7!JjhCg8RaRaeh0;G!+u{(ZR1<4LdbrMB%w z*CjvGp%hcAAdE=>)`IW&BZ8^3)_tze$m8lhj@?ONW9p^Q*L@ET z0D6#);Y&G`Z=c~b3!CLt38%iH)L$4(8Hu=Hrr!vqMEEz9GLHKH2&MFB>WZUEOyjf= z;7l3aZ`P|j$_RkY*|o=bG6bKZ6>saL>pS`VWgE?U$w+f;okDO6A^j9?7np&2x^~=? zr{WBguH~DeJCA&o3#CU+#2e?X;n%?C=V)Ng0$<8Vfvm-@t1BN1vV-+g(?vgXCC~|1 zXClzpm>hrzgm(cniq-0pfYv>(uj|wbIrN#Ih7XRZJ~N*nQoF8 zx;{NP8}eY(cifroXBNSQY;1pWr7?TBcc0dph95qX^ zGWOg^C{P}A00NgGKSZ!IzV29D`kw`w4jTJ%q5GNZKI69V*?4kb*S7G|H52ENvusRI zA?)KiZMYhNC*b>CTw9VLRSQ(34l;&Ej3k + + + + + + 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:670115158.2 %
Date:2024-01-07 21:47:14Functions:343889.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::uav_manager::UavManager::pirouetteSrv()0
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::landSrv()3
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()3
mrs_uav_managers::uav_manager::UavManager::disarmSrv()3
mrs_uav_managers::uav_manager::UavManager::ungripSrv()3
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()5
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)9
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()10
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)10
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)44
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)44
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()44
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)44
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)45
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::uav_manager::UavManager::initialize()57
mrs_uav_managers::uav_manager::UavManager::preinitialize()57
mrs_uav_managers::uav_manager::UavManager::onInit()57
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)59
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)67
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)82
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)84
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)111
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)114
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)116
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)361
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)479
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)1138
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)11159
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)11159
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)29170
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)55557
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)94450
+
+
+ + + +
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..d4503c8248 --- /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:670115158.2 %
Date:2024-01-07 21:47:14Functions:343889.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_managers::uav_manager::UavManager::initialize()57
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()10
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)44
mrs_uav_managers::uav_manager::UavManager::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::pirouetteSrv()0
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)361
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)479
mrs_uav_managers::uav_manager::UavManager::preinitialize()57
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)11159
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)11159
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)10
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)116
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)94450
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)114
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)1138
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)29170
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)55557
mrs_uav_managers::uav_manager::UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)9
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()5
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)111
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)44
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()44
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)82
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)45
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)84
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)59
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)67
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)44
mrs_uav_managers::uav_manager::UavManager::onInit()57
mrs_uav_managers::uav_manager::UavManager::landSrv()3
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()3
mrs_uav_managers::uav_manager::UavManager::disarmSrv()3
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::ungripSrv()3
+
+
+ + + +
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..7f41cc41e7 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.html @@ -0,0 +1,2733 @@ + + + + + + + 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:670115158.2 %
Date:2024-01-07 21:47:14Functions:343889.5 %
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             : 
+     139             :   // service callbacks
+     140             :   bool callbackTakeoff(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     141             :   bool callbackLand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     142             :   bool callbackLandHome(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     143             :   bool callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     144             : 
+     145             :   // service clients
+     146             :   mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>                sch_takeoff_;
+     147             :   mrs_lib::ServiceClientHandler<mrs_msgs::String>              sch_switch_tracker_;
+     148             :   mrs_lib::ServiceClientHandler<mrs_msgs::String>              sch_switch_controller_;
+     149             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_land_;
+     150             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_eland_;
+     151             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_ehover_;
+     152             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_control_callbacks_;
+     153             :   mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> sch_emergency_reference_;
+     154             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_arm_;
+     155             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_pirouette_;
+     156             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_odometry_callbacks_;
+     157             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_ungrip_;
+     158             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool>             sch_toggle_control_output_;
+     159             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger>             sch_offboard_;
+     160             : 
+     161             :   // service client wrappers
+     162             :   bool takeoffSrv(void);
+     163             :   bool switchTrackerSrv(const std::string& tracker);
+     164             :   bool switchControllerSrv(const std::string& controller);
+     165             :   bool landSrv(void);
+     166             :   bool elandSrv(void);
+     167             :   bool ehoverSrv(void);
+     168             :   void disarmSrv(void);
+     169             :   bool emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal);
+     170             :   void setOdometryCallbacksSrv(const bool& input);
+     171             :   void setControlCallbacksSrv(const bool& input);
+     172             :   void ungripSrv(void);
+     173             :   bool toggleControlOutput(const bool& input);
+     174             :   void pirouetteSrv(void);
+     175             :   bool offboardSrv(const bool in);
+     176             : 
+     177             :   ros::Timer timer_takeoff_;
+     178             :   ros::Timer timer_max_height_;
+     179             :   ros::Timer timer_min_height_;
+     180             :   ros::Timer timer_landing_;
+     181             :   ros::Timer timer_maxthrottle_;
+     182             :   ros::Timer timer_flighttime_;
+     183             :   ros::Timer timer_diagnostics_;
+     184             :   ros::Timer timer_midair_activation_;
+     185             : 
+     186             :   // timer callbacks
+     187             :   void timerLanding(const ros::TimerEvent& event);
+     188             :   void timerTakeoff(const ros::TimerEvent& event);
+     189             :   void timerMaxHeight(const ros::TimerEvent& event);
+     190             :   void timerMinHeight(const ros::TimerEvent& event);
+     191             :   void timerFlightTime(const ros::TimerEvent& event);
+     192             :   void timerMaxthrottle(const ros::TimerEvent& event);
+     193             :   void timerDiagnostics(const ros::TimerEvent& event);
+     194             : 
+     195             :   // publishers
+     196             :   mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics> ph_diag_;
+     197             : 
+     198             :   // max height checking
+     199             :   bool              _max_height_enabled_ = false;
+     200             :   double            _max_height_checking_rate_;
+     201             :   double            _max_height_offset_;
+     202             :   std::atomic<bool> fixing_max_height_ = false;
+     203             : 
+     204             :   // min height checking
+     205             :   bool              _min_height_enabled_ = false;
+     206             :   double            _min_height_checking_rate_;
+     207             :   double            _min_height_offset_;
+     208             :   double            _min_height_;
+     209             :   std::atomic<bool> fixing_min_height_ = false;
+     210             : 
+     211             :   // mass estimation during landing
+     212             :   double    throttle_mass_estimate_;
+     213             :   bool      throttle_under_threshold_ = false;
+     214             :   ros::Time throttle_mass_estimate_first_time_;
+     215             : 
+     216             :   // velocity during landing
+     217             :   bool      velocity_under_threshold_ = false;
+     218             :   ros::Time velocity_under_threshold_first_time_;
+     219             : 
+     220             :   bool _gain_manager_required_       = false;
+     221             :   bool _constraint_manager_required_ = false;
+     222             : 
+     223             :   std::tuple<bool, std::string> landImpl(void);
+     224             :   std::tuple<bool, std::string> landWithDescendImpl(void);
+     225             :   std::tuple<bool, std::string> midairActivationImpl(void);
+     226             : 
+     227             :   // saved takeoff coordinates and allows to land there again
+     228             :   mrs_msgs::ReferenceStamped land_there_reference_;
+     229             :   std::mutex                 mutex_land_there_reference_;
+     230             : 
+     231             :   // to which height to takeoff
+     232             :   double _takeoff_height_;
+     233             : 
+     234             :   std::atomic<bool> takeoff_successful_ = false;
+     235             : 
+     236             :   // names of important trackers
+     237             :   std::string _null_tracker_name_;
+     238             : 
+     239             :   // takeoff timer
+     240             :   double     _takeoff_timer_rate_;
+     241             :   bool       takingoff_            = false;
+     242             :   int        number_of_takeoffs_   = 0;
+     243             :   double     last_mass_difference_ = 0;
+     244             :   std::mutex mutex_last_mass_difference_;
+     245             :   bool       waiting_for_takeoff_ = false;
+     246             : 
+     247             :   // after takeoff
+     248             :   std::string _after_takeoff_tracker_name_;
+     249             :   std::string _after_takeoff_controller_name_;
+     250             :   std::string _takeoff_tracker_name_;
+     251             :   std::string _takeoff_controller_name_;
+     252             :   bool        _after_takeoff_pirouette_ = false;
+     253             : 
+     254             :   // Landing timer
+     255             :   std::string _landing_tracker_name_;
+     256             :   std::string _landing_controller_name_;
+     257             :   double      _landing_cutoff_mass_factor_;
+     258             :   double      _landing_cutoff_mass_timeout_;
+     259             :   double      _landing_timer_rate_;
+     260             :   double      _landing_descend_height_;
+     261             :   bool        landing_ = false;
+     262             :   double      _uav_mass_;
+     263             :   double      _g_;
+     264             :   double      landing_uav_mass_;
+     265             :   bool        _landing_disarm_ = false;
+     266             :   double      _landing_tracking_tolerance_translation_;
+     267             :   double      _landing_tracking_tolerance_heading_;
+     268             : 
+     269             :   // diagnostics timer
+     270             :   double _diagnostics_timer_rate_;
+     271             : 
+     272             :   mrs_lib::quadratic_throttle_model::MotorParams_t _throttle_model_;
+     273             : 
+     274             :   // landing state machine states
+     275             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     276             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     277             : 
+     278             :   // timer for checking max flight time
+     279             :   double     _flighttime_timer_rate_;
+     280             :   double     _flighttime_max_time_;
+     281             :   bool       _flighttime_timer_enabled_ = false;
+     282             :   double     flighttime_                = 0;
+     283             :   std::mutex mutex_flighttime_;
+     284             : 
+     285             :   // timer for checking maximum throttle
+     286             :   bool      _maxthrottle_timer_enabled_ = false;
+     287             :   double    _maxthrottle_timer_rate_;
+     288             :   double    _maxthrottle_max_throttle_;
+     289             :   double    _maxthrottle_eland_timeout_;
+     290             :   double    _maxthrottle_ungrip_timeout_;
+     291             :   bool      maxthrottle_above_threshold_ = false;
+     292             :   ros::Time maxthrottle_first_time_;
+     293             : 
+     294             :   // profiler
+     295             :   mrs_lib::Profiler profiler_;
+     296             :   bool              _profiler_enabled_ = false;
+     297             : 
+     298             :   // midair activation
+     299             :   void      timerMidairActivation(const ros::TimerEvent& event);
+     300             :   bool      callbackMidairActivation(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     301             :   ros::Time midair_activation_started_;
+     302             : 
+     303             :   double      _midair_activation_timer_rate_;
+     304             :   std::string _midair_activation_during_controller_;
+     305             :   std::string _midair_activation_during_tracker_;
+     306             :   std::string _midair_activation_after_controller_;
+     307             :   std::string _midair_activation_after_tracker_;
+     308             : 
+     309             :   void changeLandingState(LandingStates_t new_state);
+     310             : };
+     311             : 
+     312             : //}
+     313             : 
+     314             : /* onInit() //{ */
+     315             : 
+     316          57 : void UavManager::onInit() {
+     317          57 :   preinitialize();
+     318          57 : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : /* preinitialize() //{ */
+     323             : 
+     324          57 : void UavManager::preinitialize(void) {
+     325             : 
+     326          57 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     327             : 
+     328          57 :   ros::Time::waitForValid();
+     329             : 
+     330          57 :   mrs_lib::SubscribeHandlerOptions shopts;
+     331          57 :   shopts.nh                 = nh_;
+     332          57 :   shopts.node_name          = "ControlManager";
+     333          57 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     334          57 :   shopts.threadsafe         = true;
+     335          57 :   shopts.autostart          = true;
+     336          57 :   shopts.queue_size         = 10;
+     337          57 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     338             : 
+     339          57 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     340             : 
+     341          57 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &UavManager::timerHwApiCapabilities, this);
+     342          57 : }
+     343             : 
+     344             : //}
+     345             : 
+     346             : /* initialize() //{ */
+     347             : 
+     348          57 : void UavManager::initialize() {
+     349             : 
+     350          57 :   ROS_INFO("[UavManager]: initializing");
+     351             : 
+     352         114 :   mrs_lib::ParamLoader param_loader(nh_, "UavManager");
+     353             : 
+     354         114 :   std::string custom_config_path;
+     355         114 :   std::string platform_config_path;
+     356         114 :   std::string world_config_path;
+     357             : 
+     358          57 :   param_loader.loadParam("custom_config", custom_config_path);
+     359          57 :   param_loader.loadParam("platform_config", platform_config_path);
+     360          57 :   param_loader.loadParam("world_config", world_config_path);
+     361             : 
+     362          57 :   if (custom_config_path != "") {
+     363          57 :     param_loader.addYamlFile(custom_config_path);
+     364             :   }
+     365             : 
+     366          57 :   if (platform_config_path != "") {
+     367          57 :     param_loader.addYamlFile(platform_config_path);
+     368             :   }
+     369             : 
+     370          57 :   if (world_config_path != "") {
+     371          57 :     param_loader.addYamlFile(world_config_path);
+     372             :   }
+     373             : 
+     374          57 :   param_loader.addYamlFileFromParam("private_config");
+     375          57 :   param_loader.addYamlFileFromParam("public_config");
+     376             : 
+     377         114 :   const std::string yaml_prefix = "mrs_uav_managers/uav_manager/";
+     378             : 
+     379             :   // params passed from the launch file are not prefixed
+     380          57 :   param_loader.loadParam("uav_name", _uav_name_);
+     381          57 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     382          57 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     383          57 :   param_loader.loadParam("g", _g_);
+     384             : 
+     385             :   // motor params are also not prefixed, since they are common to more nodes
+     386          57 :   param_loader.loadParam("motor_params/n_motors", _throttle_model_.n_motors);
+     387          57 :   param_loader.loadParam("motor_params/a", _throttle_model_.A);
+     388          57 :   param_loader.loadParam("motor_params/b", _throttle_model_.B);
+     389             : 
+     390          57 :   param_loader.loadParam(yaml_prefix + "null_tracker", _null_tracker_name_);
+     391             : 
+     392          57 :   param_loader.loadParam(yaml_prefix + "takeoff/rate", _takeoff_timer_rate_);
+     393          57 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/tracker", _after_takeoff_tracker_name_);
+     394          57 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/controller", _after_takeoff_controller_name_);
+     395          57 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/pirouette", _after_takeoff_pirouette_);
+     396          57 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/controller", _takeoff_controller_name_);
+     397          57 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/tracker", _takeoff_tracker_name_);
+     398          57 :   param_loader.loadParam(yaml_prefix + "takeoff/takeoff_height", _takeoff_height_);
+     399             : 
+     400          57 :   param_loader.loadParam(yaml_prefix + "landing/rate", _landing_timer_rate_);
+     401          57 :   param_loader.loadParam(yaml_prefix + "landing/landing_tracker", _landing_tracker_name_);
+     402          57 :   param_loader.loadParam(yaml_prefix + "landing/landing_controller", _landing_controller_name_);
+     403          57 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_mass_factor", _landing_cutoff_mass_factor_);
+     404          57 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_timeout", _landing_cutoff_mass_timeout_);
+     405          57 :   param_loader.loadParam(yaml_prefix + "landing/disarm", _landing_disarm_);
+     406          57 :   param_loader.loadParam(yaml_prefix + "landing/descend_height", _landing_descend_height_);
+     407          57 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/translation", _landing_tracking_tolerance_translation_);
+     408          57 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/heading", _landing_tracking_tolerance_heading_);
+     409             : 
+     410          57 :   param_loader.loadParam(yaml_prefix + "midair_activation/rate", _midair_activation_timer_rate_);
+     411          57 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/controller", _midair_activation_during_controller_);
+     412          57 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/tracker", _midair_activation_during_tracker_);
+     413          57 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/controller", _midair_activation_after_controller_);
+     414          57 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/tracker", _midair_activation_after_tracker_);
+     415             : 
+     416          57 :   param_loader.loadParam(yaml_prefix + "max_height_checking/enabled", _max_height_enabled_);
+     417          57 :   param_loader.loadParam(yaml_prefix + "max_height_checking/rate", _max_height_checking_rate_);
+     418          57 :   param_loader.loadParam(yaml_prefix + "max_height_checking/safety_height_offset", _max_height_offset_);
+     419             : 
+     420          57 :   param_loader.loadParam(yaml_prefix + "min_height_checking/enabled", _min_height_enabled_);
+     421          57 :   param_loader.loadParam(yaml_prefix + "min_height_checking/rate", _min_height_checking_rate_);
+     422          57 :   param_loader.loadParam(yaml_prefix + "min_height_checking/safety_height_offset", _min_height_offset_);
+     423          57 :   param_loader.loadParam(yaml_prefix + "min_height_checking/min_height", _min_height_);
+     424             : 
+     425          57 :   param_loader.loadParam(yaml_prefix + "require_gain_manager", _gain_manager_required_);
+     426          57 :   param_loader.loadParam(yaml_prefix + "require_constraint_manager", _constraint_manager_required_);
+     427             : 
+     428          57 :   param_loader.loadParam(yaml_prefix + "flight_timer/enabled", _flighttime_timer_enabled_);
+     429          57 :   param_loader.loadParam(yaml_prefix + "flight_timer/rate", _flighttime_timer_rate_);
+     430          57 :   param_loader.loadParam(yaml_prefix + "flight_timer/max_time", _flighttime_max_time_);
+     431             : 
+     432          57 :   param_loader.loadParam(yaml_prefix + "max_throttle/enabled", _maxthrottle_timer_enabled_);
+     433          57 :   param_loader.loadParam(yaml_prefix + "max_throttle/rate", _maxthrottle_timer_rate_);
+     434          57 :   param_loader.loadParam(yaml_prefix + "max_throttle/max_throttle", _maxthrottle_max_throttle_);
+     435          57 :   param_loader.loadParam(yaml_prefix + "max_throttle/eland_timeout", _maxthrottle_eland_timeout_);
+     436          57 :   param_loader.loadParam(yaml_prefix + "max_throttle/ungrip_timeout", _maxthrottle_ungrip_timeout_);
+     437             : 
+     438          57 :   param_loader.loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_timer_rate_);
+     439             : 
+     440             :   // | ------------------- scope timer logger ------------------- |
+     441             : 
+     442          57 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     443         171 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     444          57 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     445             : 
+     446          57 :   if (!param_loader.loadedSuccessfully()) {
+     447           0 :     ROS_ERROR("[UavManager]: Could not load all parameters!");
+     448           0 :     ros::shutdown();
+     449             :   }
+     450             : 
+     451             :   // | --------------------- tf transformer --------------------- |
+     452             : 
+     453          57 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+     454          57 :   transformer_->setDefaultPrefix(_uav_name_);
+     455          57 :   transformer_->retryLookupNewest(true);
+     456             : 
+     457             :   // | ----------------------- subscribers ---------------------- |
+     458             : 
+     459         114 :   mrs_lib::SubscribeHandlerOptions shopts;
+     460          57 :   shopts.nh                 = nh_;
+     461          57 :   shopts.node_name          = "UavManager";
+     462          57 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     463          57 :   shopts.threadsafe         = true;
+     464          57 :   shopts.autostart          = true;
+     465          57 :   shopts.queue_size         = 10;
+     466          57 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     467             : 
+     468          57 :   sh_controller_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics>(shopts, "controller_diagnostics_in");
+     469          57 :   sh_odometry_               = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &UavManager::callbackOdometry, this);
+     470          57 :   sh_estimation_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "odometry_diagnostics_in");
+     471          57 :   sh_control_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     472          57 :   sh_mass_estimate_          = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "mass_estimate_in");
+     473          57 :   sh_throttle_               = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "throttle_in");
+     474          57 :   sh_height_                 = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_in");
+     475          57 :   sh_hw_api_status_          = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in");
+     476          57 :   sh_gains_diag_             = mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>(shopts, "gain_manager_diagnostics_in");
+     477          57 :   sh_constraints_diag_       = mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics>(shopts, "constraint_manager_diagnostics_in");
+     478          57 :   sh_hw_api_gnss_            = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "hw_api_gnss_in", &UavManager::callbackHwApiGNSS, this);
+     479          57 :   sh_max_height_             = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_height_in");
+     480          57 :   sh_tracker_cmd_            = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts, "tracker_cmd_in");
+     481             : 
+     482             :   // | ----------------------- publishers ----------------------- |
+     483             : 
+     484          57 :   ph_diag_ = mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     485             : 
+     486             :   // | --------------------- service servers -------------------- |
+     487             : 
+     488          57 :   service_server_takeoff_           = nh_.advertiseService("takeoff_in", &UavManager::callbackTakeoff, this);
+     489          57 :   service_server_land_              = nh_.advertiseService("land_in", &UavManager::callbackLand, this);
+     490          57 :   service_server_land_home_         = nh_.advertiseService("land_home_in", &UavManager::callbackLandHome, this);
+     491          57 :   service_server_land_there_        = nh_.advertiseService("land_there_in", &UavManager::callbackLandThere, this);
+     492          57 :   service_server_midair_activation_ = nh_.advertiseService("midair_activation_in", &UavManager::callbackMidairActivation, this);
+     493             : 
+     494             :   // | --------------------- service clients -------------------- |
+     495             : 
+     496          57 :   sch_takeoff_               = mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>(nh_, "takeoff_out");
+     497          57 :   sch_land_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "land_out");
+     498          57 :   sch_eland_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+     499          57 :   sch_ehover_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ehover_out");
+     500          57 :   sch_switch_tracker_        = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_tracker_out");
+     501          57 :   sch_switch_controller_     = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_controller_out");
+     502          57 :   sch_emergency_reference_   = mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>(nh_, "emergency_reference_out");
+     503          57 :   sch_control_callbacks_     = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "enable_callbacks_out");
+     504          57 :   sch_arm_                   = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "arm_out");
+     505          57 :   sch_pirouette_             = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "pirouette_out");
+     506          57 :   sch_odometry_callbacks_    = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+     507          57 :   sch_ungrip_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+     508          57 :   sch_toggle_control_output_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "toggle_control_output_out");
+     509          57 :   sch_offboard_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "offboard_out");
+     510             : 
+     511             :   // | ---------------------- state machine --------------------- |
+     512             : 
+     513          57 :   current_state_landing_ = IDLE_STATE;
+     514             : 
+     515             :   // | ------------------------ profiler ------------------------ |
+     516             : 
+     517          57 :   profiler_ = mrs_lib::Profiler(nh_, "UavManager", _profiler_enabled_);
+     518             : 
+     519             :   // | ------------------------- timers ------------------------- |
+     520             : 
+     521          57 :   timer_landing_           = nh_.createTimer(ros::Rate(_landing_timer_rate_), &UavManager::timerLanding, this, false, false);
+     522          57 :   timer_takeoff_           = nh_.createTimer(ros::Rate(_takeoff_timer_rate_), &UavManager::timerTakeoff, this, false, false);
+     523          57 :   timer_flighttime_        = nh_.createTimer(ros::Rate(_flighttime_timer_rate_), &UavManager::timerFlightTime, this, false, false);
+     524          57 :   timer_diagnostics_       = nh_.createTimer(ros::Rate(_diagnostics_timer_rate_), &UavManager::timerDiagnostics, this);
+     525          57 :   timer_midair_activation_ = nh_.createTimer(ros::Rate(_midair_activation_timer_rate_), &UavManager::timerMidairActivation, this, false, false);
+     526         114 :   timer_max_height_        = nh_.createTimer(ros::Rate(_max_height_checking_rate_), &UavManager::timerMaxHeight, this, false,
+     527         114 :                                       _max_height_enabled_ && hw_api_capabilities_.produces_distance_sensor);
+     528         114 :   timer_min_height_        = nh_.createTimer(ros::Rate(_min_height_checking_rate_), &UavManager::timerMinHeight, this, false,
+     529         114 :                                       _min_height_enabled_ && hw_api_capabilities_.produces_distance_sensor);
+     530             : 
+     531          54 :   bool should_check_throttle = hw_api_capabilities_.accepts_actuator_cmd || hw_api_capabilities_.accepts_control_group_cmd ||
+     532         111 :                                hw_api_capabilities_.accepts_attitude_rate_cmd || hw_api_capabilities_.accepts_attitude_cmd;
+     533             : 
+     534             :   timer_maxthrottle_ =
+     535          57 :       nh_.createTimer(ros::Rate(_maxthrottle_timer_rate_), &UavManager::timerMaxthrottle, this, false, _maxthrottle_timer_enabled_ && should_check_throttle);
+     536             : 
+     537             :   // | ----------------------- finish init ---------------------- |
+     538             : 
+     539          57 :   is_initialized_ = true;
+     540             : 
+     541          57 :   ROS_INFO("[UavManager]: initialized");
+     542             : 
+     543          57 :   ROS_DEBUG("[UavManager]: debug output is enabled");
+     544          57 : }
+     545             : 
+     546             : //}
+     547             : 
+     548             : //}
+     549             : 
+     550             : // | ---------------------- state machine --------------------- |
+     551             : 
+     552             : /* //{ changeLandingState() */
+     553             : 
+     554           9 : void UavManager::changeLandingState(LandingStates_t new_state) {
+     555             : 
+     556           9 :   previous_state_landing_ = current_state_landing_;
+     557           9 :   current_state_landing_  = new_state;
+     558             : 
+     559           9 :   switch (current_state_landing_) {
+     560             : 
+     561           3 :     case LANDING_STATE: {
+     562             : 
+     563           3 :       if (sh_mass_estimate_.hasMsg() && (ros::Time::now() - sh_mass_estimate_.lastMsgTime()).toSec() < 1.0) {
+     564             : 
+     565             :         // copy member variables
+     566           3 :         auto mass_esimtate = sh_mass_estimate_.getMsg();
+     567             : 
+     568           3 :         landing_uav_mass_ = mass_esimtate->data;
+     569             :       }
+     570             : 
+     571           3 :       break;
+     572             :     };
+     573             : 
+     574           6 :     default: {
+     575           6 :       break;
+     576             :     }
+     577             :   }
+     578             : 
+     579             :   // just for ROS_INFO
+     580           9 :   ROS_INFO("[UavManager]: Switching landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+     581           9 : }
+     582             : 
+     583             : //}
+     584             : 
+     585             : // --------------------------------------------------------------
+     586             : // |                           timers                           |
+     587             : // --------------------------------------------------------------
+     588             : 
+     589             : /* timerHwApiCapabilities() //{ */
+     590             : 
+     591          59 : void UavManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+     592             : 
+     593         118 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", 1.0, 1.0, event);
+     594         118 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+     595             : 
+     596          59 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+     597           2 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+     598           2 :     return;
+     599             :   }
+     600             : 
+     601          57 :   hw_api_capabilities_ = *sh_hw_api_capabilities_.getMsg();
+     602             : 
+     603          57 :   ROS_INFO("[ControlManager]: got HW API capabilities, initializing");
+     604             : 
+     605          57 :   initialize();
+     606             : 
+     607          57 :   timer_hw_api_capabilities_.stop();
+     608             : }
+     609             : 
+     610             : //}
+     611             : 
+     612             : /* //{ timerLanding() */
+     613             : 
+     614         361 : void UavManager::timerLanding(const ros::TimerEvent& event) {
+     615             : 
+     616         361 :   if (!is_initialized_)
+     617           0 :     return;
+     618             : 
+     619         722 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerLanding", _landing_timer_rate_, 0.1, event);
+     620         722 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerLanding", scope_timer_logger_, scope_timer_enabled_);
+     621             : 
+     622         361 :   auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+     623             : 
+     624             :   // copy member variables
+     625         361 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     626         361 :   auto odometry                    = sh_odometry_.getMsg();
+     627         361 :   auto tracker_cmd                 = sh_tracker_cmd_.getMsg();
+     628             : 
+     629         361 :   std::optional<double> desired_throttle;
+     630             : 
+     631         361 :   if (sh_throttle_.hasMsg() && (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() < 1.0) {
+     632         361 :     desired_throttle = sh_throttle_.getMsg()->data;
+     633             :   }
+     634             : 
+     635         361 :   auto res = transformer_->transformSingle(land_there_reference, odometry->header.frame_id);
+     636             : 
+     637         361 :   mrs_msgs::ReferenceStamped land_there_current_frame;
+     638             : 
+     639         361 :   if (res) {
+     640         361 :     land_there_current_frame = res.value();
+     641             :   } else {
+     642             : 
+     643           0 :     ROS_ERROR("[UavManager]: could not transform the reference into the current frame! land by yourself pls.");
+     644           0 :     return;
+     645             :   }
+     646             : 
+     647         361 :   if (current_state_landing_ == IDLE_STATE) {
+     648             : 
+     649           0 :     return;
+     650             : 
+     651         361 :   } else if (current_state_landing_ == GOTO_STATE) {
+     652             : 
+     653         107 :     auto [pos_x, pos_y, pos_z] = mrs_lib::getPosition(*tracker_cmd);
+     654         107 :     auto [ref_x, ref_y, ref_z] = mrs_lib::getPosition(land_there_current_frame);
+     655             : 
+     656         107 :     double pos_heading = tracker_cmd->heading;
+     657             : 
+     658         107 :     double ref_heading = 0;
+     659             :     try {
+     660         107 :       ref_heading = mrs_lib::getHeading(land_there_current_frame);
+     661             :     }
+     662           0 :     catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     663           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     664           0 :       return;
+     665             :     }
+     666             : 
+     667         110 :     if (mrs_lib::geometry::dist(vec3_t(pos_x, pos_y, pos_z), vec3_t(ref_x, ref_y, ref_z)) < _landing_tracking_tolerance_translation_ &&
+     668           3 :         fabs(radians::diff(pos_heading, ref_heading)) < _landing_tracking_tolerance_heading_) {
+     669             : 
+     670           6 :       auto [success, message] = landWithDescendImpl();
+     671             : 
+     672           3 :       if (!success) {
+     673             : 
+     674           0 :         ROS_ERROR_THROTTLE(1.0, "[UavManager]: call for landing failed: '%s'", message.c_str());
+     675             :       }
+     676             : 
+     677         104 :     } else if (!control_manager_diagnostics->tracker_status.have_goal && control_manager_diagnostics->flying_normally) {
+     678             : 
+     679           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: the tracker does not have a goal while flying home, setting the reference again");
+     680             : 
+     681           0 :       mrs_msgs::ReferenceStamped reference_out;
+     682             : 
+     683             :       {
+     684           0 :         std::scoped_lock lock(mutex_land_there_reference_);
+     685             : 
+     686             :         // get the current altitude in land_there_reference_.header.frame_id;
+     687           0 :         geometry_msgs::PoseStamped current_pose;
+     688           0 :         current_pose.header.stamp     = ros::Time::now();
+     689           0 :         current_pose.header.frame_id  = _uav_name_ + "/fcu";
+     690           0 :         current_pose.pose.position.x  = 0;
+     691           0 :         current_pose.pose.position.y  = 0;
+     692           0 :         current_pose.pose.position.z  = 0;
+     693           0 :         current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     694             : 
+     695           0 :         auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+     696             : 
+     697           0 :         if (response) {
+     698             : 
+     699           0 :           land_there_reference_.reference.position.z = response.value().pose.position.z;
+     700           0 :           ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+     701             : 
+     702             :         } else {
+     703             : 
+     704           0 :           std::stringstream ss;
+     705           0 :           ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+     706           0 :           ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+     707             :         }
+     708             : 
+     709           0 :         reference_out.header.frame_id = land_there_reference_.header.frame_id;
+     710           0 :         reference_out.header.stamp    = ros::Time::now();
+     711           0 :         reference_out.reference       = land_there_reference_.reference;
+     712             :       }
+     713             : 
+     714           0 :       emergencyReferenceSrv(reference_out);
+     715             :     }
+     716             : 
+     717         254 :   } else if (current_state_landing_ == LANDING_STATE) {
+     718             : 
+     719             :     // we should not attempt to finish the landing if some other tracked was activated
+     720         254 :     if (_landing_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+     721             : 
+     722         254 :       if (desired_throttle) {
+     723             : 
+     724             :         // recalculate the mass based on the throttle
+     725         254 :         throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(_throttle_model_, desired_throttle.value()) / _g_;
+     726         254 :         ROS_INFO_THROTTLE(1.0, "[UavManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+     727             : 
+     728             :         // condition for automatic motor turn off
+     729         254 :         if (((throttle_mass_estimate_ < _landing_cutoff_mass_factor_ * landing_uav_mass_) || desired_throttle < 0.01)) {
+     730             : 
+     731          66 :           if (!throttle_under_threshold_) {
+     732             : 
+     733           3 :             throttle_mass_estimate_first_time_ = ros::Time::now();
+     734           3 :             throttle_under_threshold_          = true;
+     735             :           }
+     736             : 
+     737          66 :           ROS_INFO_THROTTLE(0.5, "[UavManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+     738             : 
+     739             :         } else {
+     740             : 
+     741         188 :           throttle_under_threshold_ = false;
+     742             :         }
+     743             : 
+     744         254 :         if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _landing_cutoff_mass_timeout_)) {
+     745             : 
+     746           3 :           switchTrackerSrv(_null_tracker_name_);
+     747             : 
+     748           3 :           setControlCallbacksSrv(true);
+     749             : 
+     750           3 :           if (_landing_disarm_) {
+     751             : 
+     752           3 :             ROS_INFO("[UavManager]: disarming after landing");
+     753             : 
+     754           3 :             disarmSrv();
+     755             :           }
+     756             : 
+     757           3 :           changeLandingState(IDLE_STATE);
+     758             : 
+     759           3 :           ROS_INFO("[UavManager]: landing finished");
+     760             : 
+     761           3 :           timer_landing_.stop();
+     762             :         }
+     763             : 
+     764             :       } else {
+     765             : 
+     766           0 :         auto odometry = sh_odometry_.getMsg();
+     767             : 
+     768           0 :         double z_vel = odometry->twist.twist.linear.z;
+     769             : 
+     770           0 :         ROS_INFO_THROTTLE(1.0, "[UavManager]: landing: z-velocity: %.2f", z_vel);
+     771             : 
+     772             :         // condition for automatic motor turn off
+     773           0 :         if (z_vel > -0.1) {
+     774             : 
+     775           0 :           if (!velocity_under_threshold_) {
+     776             : 
+     777           0 :             velocity_under_threshold_first_time_ = ros::Time::now();
+     778           0 :             velocity_under_threshold_            = true;
+     779             :           }
+     780             : 
+     781           0 :           ROS_INFO_THROTTLE(0.5, "[UavManager]: velocity over threshold for %.2f s", (ros::Time::now() - velocity_under_threshold_first_time_).toSec());
+     782             : 
+     783             :         } else {
+     784             : 
+     785           0 :           velocity_under_threshold_ = false;
+     786             :         }
+     787             : 
+     788           0 :         if (velocity_under_threshold_ && ((ros::Time::now() - velocity_under_threshold_first_time_).toSec() > 3.0)) {
+     789             : 
+     790           0 :           switchTrackerSrv(_null_tracker_name_);
+     791             : 
+     792           0 :           setControlCallbacksSrv(true);
+     793             : 
+     794           0 :           if (_landing_disarm_) {
+     795             : 
+     796           0 :             ROS_INFO("[UavManager]: disarming after landing");
+     797             : 
+     798           0 :             disarmSrv();
+     799             :           }
+     800             : 
+     801           0 :           changeLandingState(IDLE_STATE);
+     802             : 
+     803           0 :           ROS_INFO("[UavManager]: landing finished");
+     804             : 
+     805           0 :           timer_landing_.stop();
+     806             :         }
+     807             :       }
+     808             : 
+     809             :     } else {
+     810             : 
+     811           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: incorrect tracker detected during landing!");
+     812             :     }
+     813             :   }
+     814             : }
+     815             : 
+     816             : //}
+     817             : 
+     818             : /* //{ timerTakeoff() */
+     819             : 
+     820         479 : void UavManager::timerTakeoff(const ros::TimerEvent& event) {
+     821             : 
+     822         479 :   if (!is_initialized_)
+     823           0 :     return;
+     824             : 
+     825         958 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerTakeoff", _takeoff_timer_rate_, 0.1, event);
+     826         958 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerTakeoff", scope_timer_logger_, scope_timer_enabled_);
+     827             : 
+     828         479 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     829             : 
+     830         479 :   if (waiting_for_takeoff_) {
+     831             : 
+     832          10 :     if (control_manager_diagnostics->active_tracker == _takeoff_tracker_name_ && control_manager_diagnostics->tracker_status.have_goal) {
+     833             : 
+     834          10 :       waiting_for_takeoff_ = false;
+     835             :     } else {
+     836             : 
+     837           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: waiting for takeoff confirmation from the ControlManager");
+     838           0 :       return;
+     839             :     }
+     840             :   }
+     841             : 
+     842         479 :   if (takingoff_) {
+     843             : 
+     844         479 :     if (control_manager_diagnostics->active_tracker != _takeoff_tracker_name_ || !control_manager_diagnostics->tracker_status.have_goal) {
+     845             : 
+     846          10 :       ROS_INFO("[UavManager]: take off finished, switching to %s", _after_takeoff_tracker_name_.c_str());
+     847             : 
+     848          10 :       switchTrackerSrv(_after_takeoff_tracker_name_);
+     849             : 
+     850          10 :       switchControllerSrv(_after_takeoff_controller_name_);
+     851             : 
+     852          10 :       setOdometryCallbacksSrv(true);
+     853             : 
+     854          10 :       if (_after_takeoff_pirouette_) {
+     855             : 
+     856           0 :         pirouetteSrv();
+     857             :       }
+     858             : 
+     859          10 :       timer_takeoff_.stop();
+     860             :     }
+     861             :   }
+     862             : }
+     863             : 
+     864             : //}
+     865             : 
+     866             : /* //{ timerMaxHeight() */
+     867             : 
+     868       11159 : void UavManager::timerMaxHeight(const ros::TimerEvent& event) {
+     869             : 
+     870       11159 :   if (!is_initialized_)
+     871        6747 :     return;
+     872             : 
+     873       22318 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxHeight", _max_height_checking_rate_, 0.1, event);
+     874       22318 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxHeight", scope_timer_logger_, scope_timer_enabled_);
+     875             : 
+     876       11159 :   if (!sh_max_height_.hasMsg() || !sh_height_.hasMsg() || !sh_odometry_.hasMsg()) {
+     877        3745 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: maxHeightTimer() not spinning, missing data");
+     878        3745 :     return;
+     879             :   }
+     880             : 
+     881        7414 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     882             : 
+     883        7414 :   if (!fixing_max_height_ && !control_manager_diag->flying_normally) {
+     884        3002 :     return;
+     885             :   }
+     886             : 
+     887        4412 :   auto   odometry = sh_odometry_.getMsg();
+     888        4412 :   double height   = sh_height_.getMsg()->value;
+     889             : 
+     890             :   // transform max z to the height frame
+     891        4412 :   geometry_msgs::PointStamped point;
+     892        4412 :   point.header  = sh_max_height_.getMsg()->header;
+     893        4412 :   point.point.z = sh_max_height_.getMsg()->value;
+     894             : 
+     895        4412 :   auto res = transformer_->transformSingle(point, sh_height_.getMsg()->header.frame_id);
+     896             : 
+     897             :   double max_z_in_height;
+     898             : 
+     899        4412 :   if (res) {
+     900        4412 :     max_z_in_height = res->point.z;
+     901             :   } else {
+     902           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: timerMaxHeight() not working, cannot transform max z to the height frame");
+     903           0 :     return;
+     904             :   }
+     905             : 
+     906        4412 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     907             : 
+     908        4412 :   double odometry_heading = 0;
+     909             :   try {
+     910        4412 :     odometry_heading = mrs_lib::getHeading(odometry);
+     911             :   }
+     912           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     913           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     914           0 :     return;
+     915             :   }
+     916             : 
+     917        4412 :   if (height > max_z_in_height) {
+     918             : 
+     919          53 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: max height exceeded: %.2f >  %.2f, triggering safety goto", height, max_z_in_height);
+     920             : 
+     921         106 :     mrs_msgs::ReferenceStamped reference_out;
+     922          53 :     reference_out.header.frame_id = odometry->header.frame_id;
+     923          53 :     reference_out.header.stamp    = ros::Time::now();
+     924             : 
+     925          53 :     reference_out.reference.position.x = odometry_x;
+     926          53 :     reference_out.reference.position.y = odometry_y;
+     927          53 :     reference_out.reference.position.z = odometry_z + ((max_z_in_height - _max_height_offset_) - height);
+     928             : 
+     929          53 :     reference_out.reference.heading = odometry_heading;
+     930             : 
+     931          53 :     setControlCallbacksSrv(false);
+     932             : 
+     933          53 :     bool success = emergencyReferenceSrv(reference_out);
+     934             : 
+     935          53 :     if (success) {
+     936             : 
+     937          53 :       ROS_INFO("[UavManager]: descending");
+     938             : 
+     939          53 :       fixing_max_height_ = true;
+     940             : 
+     941             :     } else {
+     942             : 
+     943           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not descend");
+     944             : 
+     945           0 :       setControlCallbacksSrv(true);
+     946             :     }
+     947             :   }
+     948             : 
+     949        4412 :   if (fixing_max_height_ && height < max_z_in_height) {
+     950             : 
+     951           1 :     setControlCallbacksSrv(true);
+     952             : 
+     953           1 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+     954             : 
+     955           1 :     fixing_max_height_ = false;
+     956             :   }
+     957             : }
+     958             : 
+     959             : //}
+     960             : 
+     961             : /* //{ timerMinHeight() */
+     962             : 
+     963       11159 : void UavManager::timerMinHeight(const ros::TimerEvent& event) {
+     964             : 
+     965       11159 :   if (!is_initialized_)
+     966        6775 :     return;
+     967             : 
+     968       11159 :   ROS_INFO_ONCE("[UavManager]: min height timer spinning");
+     969             : 
+     970       22318 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMinHeight", _min_height_checking_rate_, 0.1, event);
+     971       22318 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMinHeight", scope_timer_logger_, scope_timer_enabled_);
+     972             : 
+     973       11159 :   if (!sh_odometry_.hasMsg() || !sh_height_.hasMsg() || !sh_control_manager_diag_.hasMsg()) {
+     974        3735 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: minHeightTimer() not spinning, missing data");
+     975        3735 :     return;
+     976             :   }
+     977             : 
+     978        7424 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     979             : 
+     980        7424 :   if (!fixing_min_height_ && !control_manager_diag->flying_normally) {
+     981        3040 :     return;
+     982             :   }
+     983             : 
+     984        4384 :   auto   odometry = sh_odometry_.getMsg();
+     985        4384 :   double height   = sh_height_.getMsg()->value;
+     986             : 
+     987        4384 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     988             : 
+     989        4384 :   double odometry_heading = 0;
+     990             : 
+     991             :   try {
+     992        4384 :     odometry_heading = mrs_lib::getHeading(odometry);
+     993             :   }
+     994           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     995           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     996           0 :     return;
+     997             :   }
+     998             : 
+     999        4384 :   if (height < _min_height_) {
+    1000             : 
+    1001          26 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: min height breached: %.2f < %.2f, triggering safety goto", height, _min_height_);
+    1002             : 
+    1003          52 :     mrs_msgs::ReferenceStamped reference_out;
+    1004          26 :     reference_out.header.frame_id = odometry->header.frame_id;
+    1005          26 :     reference_out.header.stamp    = ros::Time::now();
+    1006             : 
+    1007          26 :     reference_out.reference.position.x = odometry_x;
+    1008          26 :     reference_out.reference.position.y = odometry_y;
+    1009          26 :     reference_out.reference.position.z = odometry_z + ((_min_height_ + _min_height_offset_) - height);
+    1010             : 
+    1011          26 :     reference_out.reference.heading = odometry_heading;
+    1012             : 
+    1013          26 :     setControlCallbacksSrv(false);
+    1014             : 
+    1015          26 :     bool success = emergencyReferenceSrv(reference_out);
+    1016             : 
+    1017          26 :     if (success) {
+    1018             : 
+    1019          26 :       ROS_INFO("[UavManager]: ascending");
+    1020             : 
+    1021          26 :       fixing_min_height_ = true;
+    1022             : 
+    1023             :     } else {
+    1024             : 
+    1025           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not ascend");
+    1026             : 
+    1027           0 :       setControlCallbacksSrv(true);
+    1028             :     }
+    1029             :   }
+    1030             : 
+    1031        4384 :   if (fixing_min_height_ && height > _min_height_) {
+    1032             : 
+    1033           1 :     setControlCallbacksSrv(true);
+    1034             : 
+    1035           1 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+    1036             : 
+    1037           1 :     fixing_min_height_ = false;
+    1038             :   }
+    1039             : }
+    1040             : 
+    1041             : //}
+    1042             : 
+    1043             : /* //{ timerFlightTime() */
+    1044             : 
+    1045         116 : void UavManager::timerFlightTime(const ros::TimerEvent& event) {
+    1046             : 
+    1047         116 :   if (!is_initialized_)
+    1048           0 :     return;
+    1049             : 
+    1050         348 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFlightTime", _flighttime_timer_rate_, 0.1, event);
+    1051         348 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerFlightTime", scope_timer_logger_, scope_timer_enabled_);
+    1052             : 
+    1053         116 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1054             : 
+    1055         116 :   flighttime += 1.0 / _flighttime_timer_rate_;
+    1056             : 
+    1057         116 :   mrs_msgs::Float64 flight_time;
+    1058         116 :   flight_time.value = flighttime;
+    1059             : 
+    1060             :   // if enabled, start the timer for measuring the flight time
+    1061         116 :   if (_flighttime_timer_enabled_) {
+    1062             : 
+    1063           0 :     if (flighttime > _flighttime_max_time_) {
+    1064             : 
+    1065           0 :       flighttime = 0;
+    1066           0 :       timer_flighttime_.stop();
+    1067             : 
+    1068           0 :       ROS_INFO("[UavManager]: max flight time reached, landing");
+    1069             : 
+    1070           0 :       landImpl();
+    1071             :     }
+    1072             :   }
+    1073             : 
+    1074         116 :   mrs_lib::set_mutexed(mutex_flighttime_, flighttime, flighttime_);
+    1075             : }
+    1076             : 
+    1077             : //}
+    1078             : 
+    1079             : /* //{ timerMaxthrottle() */
+    1080             : 
+    1081       29170 : void UavManager::timerMaxthrottle(const ros::TimerEvent& event) {
+    1082             : 
+    1083       29170 :   if (!is_initialized_)
+    1084        7637 :     return;
+    1085             : 
+    1086       29170 :   if (!sh_throttle_.hasMsg() || (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() > 1.0) {
+    1087        7637 :     return;
+    1088             :   }
+    1089             : 
+    1090       21533 :   ROS_INFO_ONCE("[UavManager]: max throttle timer spinning");
+    1091             : 
+    1092       64599 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxthrottle", _maxthrottle_timer_rate_, 0.03, event);
+    1093       64599 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxthrottle", scope_timer_logger_, scope_timer_enabled_);
+    1094             : 
+    1095       21533 :   auto desired_throttle = sh_throttle_.getMsg()->data;
+    1096             : 
+    1097       21533 :   if (desired_throttle >= _maxthrottle_max_throttle_) {
+    1098             : 
+    1099           0 :     if (!maxthrottle_above_threshold_) {
+    1100             : 
+    1101           0 :       maxthrottle_first_time_      = ros::Time::now();
+    1102           0 :       maxthrottle_above_threshold_ = true;
+    1103           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: max throttle exceeded threshold (%.2f/%.2f)", desired_throttle, _maxthrottle_max_throttle_);
+    1104             : 
+    1105             :     } else {
+    1106             : 
+    1107           0 :       ROS_WARN_THROTTLE(0.1, "[UavManager]: throttle over threshold (%.2f/%.2f) for %.2f s", desired_throttle, _maxthrottle_max_throttle_,
+    1108             :                         (ros::Time::now() - maxthrottle_first_time_).toSec());
+    1109             :     }
+    1110             : 
+    1111             :   } else {
+    1112             : 
+    1113       21533 :     maxthrottle_above_threshold_ = false;
+    1114             :   }
+    1115             : 
+    1116       21533 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_ungrip_timeout_) {
+    1117             : 
+    1118           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, ungripping payload", desired_throttle,
+    1119             :                       _maxthrottle_max_throttle_, _maxthrottle_ungrip_timeout_);
+    1120             : 
+    1121           0 :     ungripSrv();
+    1122             :   }
+    1123             : 
+    1124       21533 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_eland_timeout_) {
+    1125             : 
+    1126           0 :     timer_maxthrottle_.stop();
+    1127             : 
+    1128           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, calling for emergency landing", desired_throttle,
+    1129             :                        _maxthrottle_max_throttle_, _maxthrottle_eland_timeout_);
+    1130             : 
+    1131           0 :     elandSrv();
+    1132             :   }
+    1133             : }
+    1134             : 
+    1135             : //}
+    1136             : 
+    1137             : /* //{ timerDiagnostics() */
+    1138             : 
+    1139        1138 : void UavManager::timerDiagnostics(const ros::TimerEvent& event) {
+    1140             : 
+    1141        1138 :   if (!is_initialized_)
+    1142           0 :     return;
+    1143             : 
+    1144        3414 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _maxthrottle_timer_rate_, 0.03, event);
+    1145        3414 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    1146             : 
+    1147        1138 :   bool got_gps_est = false;
+    1148        1138 :   bool got_rtk_est = false;
+    1149             : 
+    1150        1138 :   if (sh_estimation_diagnostics_.hasMsg()) {  // get current position in lat-lon
+    1151             : 
+    1152        1834 :     auto                     estimation_diag  = sh_estimation_diagnostics_.getMsg();
+    1153         917 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    1154             : 
+    1155         978 :     got_gps_est = std::find(state_estimators.begin(), state_estimators.end(), "gps_garmin") != state_estimators.end() ||
+    1156          61 :                   std::find(state_estimators.begin(), state_estimators.end(), "gps_baro") != state_estimators.end();
+    1157         917 :     got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    1158             :   }
+    1159             : 
+    1160        2276 :   mrs_msgs::UavManagerDiagnostics diag;
+    1161             : 
+    1162        1138 :   diag.stamp    = ros::Time::now();
+    1163        1138 :   diag.uav_name = _uav_name_;
+    1164             : 
+    1165        1138 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1166             : 
+    1167             :   // fill in the acumulated flight time
+    1168        1138 :   diag.flight_time = flighttime;
+    1169             : 
+    1170        1138 :   if (sh_odometry_.hasMsg()) {  // get current position in lat-lon
+    1171             : 
+    1172         922 :     if (got_gps_est || got_rtk_est) {
+    1173             : 
+    1174        1776 :       nav_msgs::Odometry odom = *sh_odometry_.getMsg();
+    1175             : 
+    1176        1776 :       geometry_msgs::PoseStamped uav_pose;
+    1177         888 :       uav_pose.pose = mrs_lib::getPose(odom);
+    1178             : 
+    1179        2664 :       auto res = transformer_->transformSingle(uav_pose, "latlon_origin");
+    1180             : 
+    1181         888 :       if (res) {
+    1182         888 :         diag.cur_latitude  = res.value().pose.position.x;
+    1183         888 :         diag.cur_longitude = res.value().pose.position.y;
+    1184             :       }
+    1185             :     }
+    1186             :   }
+    1187             : 
+    1188        1138 :   if (takeoff_successful_) {
+    1189             : 
+    1190         135 :     if (got_gps_est || got_rtk_est) {
+    1191             : 
+    1192         270 :       auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+    1193             : 
+    1194         405 :       auto res = transformer_->transformSingle(land_there_reference, "latlon_origin");
+    1195             : 
+    1196         135 :       if (res) {
+    1197         135 :         diag.home_latitude  = res.value().reference.position.x;
+    1198         135 :         diag.home_longitude = res.value().reference.position.y;
+    1199             :       }
+    1200             :     }
+    1201             :   }
+    1202             : 
+    1203        1138 :   ph_diag_.publish(diag);
+    1204             : }
+    1205             : 
+    1206             : //}
+    1207             : 
+    1208             : /* //{ timerMidairActivation() */
+    1209             : 
+    1210          45 : void UavManager::timerMidairActivation([[maybe_unused]] const ros::TimerEvent& event) {
+    1211             : 
+    1212          45 :   if (!is_initialized_)
+    1213           0 :     return;
+    1214             : 
+    1215          45 :   ROS_INFO_THROTTLE(0.1, "[UavManager]: waiting for OFFBOARD");
+    1216             : 
+    1217          45 :   if (sh_hw_api_status_.getMsg()->offboard) {
+    1218             : 
+    1219          44 :     ROS_INFO("[UavManager]: OFFBOARD detected");
+    1220             : 
+    1221          44 :     setOdometryCallbacksSrv(true);
+    1222             : 
+    1223             :     {
+    1224          44 :       bool controller_switched = switchControllerSrv(_midair_activation_after_controller_);
+    1225             : 
+    1226          44 :       if (!controller_switched) {
+    1227             : 
+    1228           0 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_controller_.c_str());
+    1229             : 
+    1230           0 :         ehoverSrv();
+    1231             : 
+    1232           0 :         timer_midair_activation_.stop();
+    1233             : 
+    1234           0 :         return;
+    1235             :       }
+    1236             :     }
+    1237             : 
+    1238             :     {
+    1239          44 :       bool tracker_switched = switchTrackerSrv(_midair_activation_after_tracker_);
+    1240             : 
+    1241          44 :       if (!tracker_switched) {
+    1242             : 
+    1243           0 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_tracker_.c_str());
+    1244             : 
+    1245           0 :         ehoverSrv();
+    1246             : 
+    1247           0 :         timer_midair_activation_.stop();
+    1248             : 
+    1249           0 :         return;
+    1250             :       }
+    1251             :     }
+    1252             : 
+    1253          44 :     timer_midair_activation_.stop();
+    1254             : 
+    1255          44 :     return;
+    1256             :   }
+    1257             : 
+    1258           1 :   if ((ros::Time::now() - midair_activation_started_).toSec() > 0.5) {
+    1259             : 
+    1260           0 :     ROS_ERROR("[UavManager]: waiting for OFFBOARD timeouted, reverting");
+    1261             : 
+    1262           0 :     toggleControlOutput(false);
+    1263             : 
+    1264           0 :     timer_midair_activation_.stop();
+    1265             : 
+    1266           0 :     return;
+    1267             :   }
+    1268             : }
+    1269             : 
+    1270             : //}
+    1271             : 
+    1272             : // --------------------------------------------------------------
+    1273             : // |                          callbacks                         |
+    1274             : // --------------------------------------------------------------
+    1275             : 
+    1276             : // | --------------------- topic callbacks -------------------- |
+    1277             : 
+    1278             : /* //{ callbackHwApiGNSS() */
+    1279             : 
+    1280       55557 : void UavManager::callbackHwApiGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    1281             : 
+    1282       55557 :   if (!is_initialized_)
+    1283          23 :     return;
+    1284             : 
+    1285      166602 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiGNSS");
+    1286      166602 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::callbackHwApiGNSS", scope_timer_logger_, scope_timer_enabled_);
+    1287             : 
+    1288       55534 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    1289             : }
+    1290             : 
+    1291             : //}
+    1292             : 
+    1293             : /* //{ callbackOdometry() */
+    1294             : 
+    1295       94450 : void UavManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    1296             : 
+    1297       94450 :   if (!is_initialized_)
+    1298           0 :     return;
+    1299             : 
+    1300       94450 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    1301             : }
+    1302             : 
+    1303             : //}
+    1304             : 
+    1305             : // | -------------------- service callbacks ------------------- |
+    1306             : 
+    1307             : /* //{ callbackTakeoff() */
+    1308             : 
+    1309          10 : bool UavManager::callbackTakeoff([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1310             : 
+    1311          10 :   if (!is_initialized_)
+    1312           0 :     return false;
+    1313             : 
+    1314          10 :   ROS_INFO("[UavManager]: takeoff called by service");
+    1315             : 
+    1316             :   /* preconditions //{ */
+    1317             : 
+    1318             :   {
+    1319          10 :     std::stringstream ss;
+    1320             : 
+    1321          10 :     if (!sh_odometry_.hasMsg()) {
+    1322           0 :       ss << "can not takeoff, missing odometry!";
+    1323           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1324           0 :       res.message = ss.str();
+    1325           0 :       res.success = false;
+    1326           0 :       return true;
+    1327             :     }
+    1328             : 
+    1329          10 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1330           0 :       ss << "can not takeoff, missing HW API status!";
+    1331           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1332           0 :       res.message = ss.str();
+    1333           0 :       res.success = false;
+    1334           0 :       return true;
+    1335             :     }
+    1336             : 
+    1337          10 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1338           0 :       ss << "can not takeoff, UAV not armed!";
+    1339           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1340           0 :       res.message = ss.str();
+    1341           0 :       res.success = false;
+    1342           0 :       return true;
+    1343             :     }
+    1344             : 
+    1345          10 :     if (!sh_hw_api_status_.getMsg()->offboard) {
+    1346           0 :       ss << "can not takeoff, UAV not in offboard mode!";
+    1347           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1348           0 :       res.message = ss.str();
+    1349           0 :       res.success = false;
+    1350           0 :       return true;
+    1351             :     }
+    1352             : 
+    1353             :     {
+    1354          10 :       if (!sh_control_manager_diag_.hasMsg() && (ros::Time::now() - sh_control_manager_diag_.lastMsgTime()).toSec() > 5.0) {
+    1355           0 :         ss << "can not takeoff, missing control manager diagnostics!";
+    1356           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1357           0 :         res.message = ss.str();
+    1358           0 :         res.success = false;
+    1359           0 :         return true;
+    1360             :       }
+    1361             : 
+    1362          10 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1363           0 :         ss << "can not takeoff, need '" << _null_tracker_name_ << "' to be active!";
+    1364           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1365           0 :         res.message = ss.str();
+    1366           0 :         res.success = false;
+    1367           0 :         return true;
+    1368             :       }
+    1369             :     }
+    1370             : 
+    1371          10 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1372           0 :       ss << "can not takeoff, missing controller diagnostics!";
+    1373           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1374           0 :       res.message = ss.str();
+    1375           0 :       res.success = false;
+    1376           0 :       return true;
+    1377             :     }
+    1378             : 
+    1379          10 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1380           0 :       ss << "can not takeoff, GainManager is not running!";
+    1381           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1382           0 :       res.message = ss.str();
+    1383           0 :       res.success = false;
+    1384           0 :       return true;
+    1385             :     }
+    1386             : 
+    1387          10 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    1388           0 :       ss << "can not takeoff, ConstraintManager is not running!";
+    1389           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1390           0 :       res.message = ss.str();
+    1391           0 :       res.success = false;
+    1392           0 :       return true;
+    1393             :     }
+    1394             : 
+    1395          10 :     if (!sh_control_manager_diag_.getMsg()->output_enabled) {
+    1396             : 
+    1397           0 :       ss << "can not takeoff, Control Manager's output is disabled!";
+    1398           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1399           0 :       res.message = ss.str();
+    1400           0 :       res.success = false;
+    1401           0 :       return true;
+    1402             :     }
+    1403             : 
+    1404          10 :     if (number_of_takeoffs_ > 0) {
+    1405             : 
+    1406           0 :       auto last_mass_difference = mrs_lib::get_mutexed(mutex_last_mass_difference_, last_mass_difference_);
+    1407             : 
+    1408           0 :       if (last_mass_difference > 1.0) {
+    1409             : 
+    1410           0 :         ss << std::setprecision(2);
+    1411           0 :         ss << "can not takeoff, estimated mass difference is too large: " << _null_tracker_name_ << "!";
+    1412           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1413           0 :         res.message = ss.str();
+    1414           0 :         res.success = false;
+    1415           0 :         return true;
+    1416             :       }
+    1417             :     }
+    1418             :   }
+    1419             : 
+    1420             :   //}
+    1421             : 
+    1422          20 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+    1423          20 :   auto odometry                    = sh_odometry_.getMsg();
+    1424          10 :   auto [odom_x, odom_y, odom_z]    = mrs_lib::getPosition(sh_odometry_.getMsg());
+    1425             : 
+    1426             :   double odom_heading;
+    1427             :   try {
+    1428          10 :     odom_heading = mrs_lib::getHeading(sh_odometry_.getMsg());
+    1429             :   }
+    1430           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    1431           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+    1432             : 
+    1433           0 :     std::stringstream ss;
+    1434           0 :     ss << "could not calculate current heading";
+    1435           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1436           0 :     res.message = ss.str();
+    1437           0 :     res.success = false;
+    1438           0 :     return true;
+    1439             :   }
+    1440             : 
+    1441          10 :   ROS_INFO("[UavManager]: taking off");
+    1442             : 
+    1443          10 :   setOdometryCallbacksSrv(false);
+    1444             : 
+    1445             :   // activating the takeoff controller
+    1446             :   {
+    1447          10 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    1448          10 :     bool        controller_switched = switchControllerSrv(_takeoff_controller_name_);
+    1449             : 
+    1450             :     // if it fails, activate back the old controller
+    1451             :     // this is no big deal since the control outputs are not used
+    1452             :     // until NullTracker is active
+    1453          10 :     if (!controller_switched) {
+    1454             : 
+    1455           0 :       std::stringstream ss;
+    1456           0 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for takeoff";
+    1457           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1458           0 :       res.success = false;
+    1459           0 :       res.message = ss.str();
+    1460             : 
+    1461           0 :       switchControllerSrv(old_controller);
+    1462             : 
+    1463           0 :       return true;
+    1464             :     }
+    1465             :   }
+    1466             : 
+    1467             :   // activate the takeoff tracker
+    1468             :   {
+    1469          10 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    1470          10 :     bool        tracker_switched = switchTrackerSrv(_takeoff_tracker_name_);
+    1471             : 
+    1472             :     // if it fails, activate back the old tracker
+    1473          10 :     if (!tracker_switched) {
+    1474             : 
+    1475           0 :       std::stringstream ss;
+    1476           0 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for takeoff";
+    1477           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1478           0 :       res.success = false;
+    1479           0 :       res.message = ss.str();
+    1480             : 
+    1481           0 :       switchTrackerSrv(old_tracker);
+    1482             : 
+    1483           0 :       return true;
+    1484             :     }
+    1485             :   }
+    1486             : 
+    1487             :   // let's sleep before calling take off.. the motors are rumping-up anyway
+    1488             :   // this solves on-time-happening race condition in the landoff tracker
+    1489          10 :   ros::Duration(0.3).sleep();
+    1490             : 
+    1491             :   // now the takeoff tracker and controller are active
+    1492             :   // the UAV is basically hovering on the ground
+    1493             :   // (the controller is probably rumping up the throttle now)
+    1494             : 
+    1495             :   // call the takeoff service at the takeoff tracker
+    1496             :   {
+    1497          10 :     bool takeoff_successful = takeoffSrv();
+    1498             : 
+    1499             :     // if the takeoff was not successful, switch to NullTracker
+    1500          10 :     if (takeoff_successful) {
+    1501             : 
+    1502             :       // save the current spot for later landing
+    1503             :       {
+    1504          10 :         std::scoped_lock lock(mutex_land_there_reference_);
+    1505             : 
+    1506          10 :         land_there_reference_.header               = odometry->header;
+    1507          10 :         land_there_reference_.reference.position.x = odom_x;
+    1508          10 :         land_there_reference_.reference.position.y = odom_y;
+    1509          10 :         land_there_reference_.reference.position.z = odom_z;
+    1510          10 :         land_there_reference_.reference.heading    = odom_heading;
+    1511             :       }
+    1512             : 
+    1513          10 :       timer_flighttime_.start();
+    1514             : 
+    1515          20 :       std::stringstream ss;
+    1516          10 :       ss << "taking off";
+    1517          10 :       res.success = true;
+    1518          10 :       res.message = ss.str();
+    1519          10 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1520             : 
+    1521          10 :       takingoff_ = true;
+    1522          10 :       number_of_takeoffs_++;
+    1523          10 :       waiting_for_takeoff_ = true;
+    1524             : 
+    1525             :       // start the takeoff timer
+    1526          10 :       timer_takeoff_.start();
+    1527             : 
+    1528          10 :       takeoff_successful_ = takeoff_successful;
+    1529             : 
+    1530             :     } else {
+    1531             : 
+    1532           0 :       std::stringstream ss;
+    1533           0 :       ss << "takeoff was not successful";
+    1534           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1535           0 :       res.success = false;
+    1536           0 :       res.message = ss.str();
+    1537             : 
+    1538             :       // if the call for takeoff fails, call for emergency landing
+    1539           0 :       elandSrv();
+    1540             :     }
+    1541             :   }
+    1542             : 
+    1543          10 :   return true;
+    1544             : }
+    1545             : 
+    1546             : //}
+    1547             : 
+    1548             : /* //{ callbackLand() */
+    1549             : 
+    1550           2 : bool UavManager::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1551             : 
+    1552           2 :   if (!is_initialized_)
+    1553           0 :     return false;
+    1554             : 
+    1555           2 :   ROS_INFO("[UavManager]: land called by service");
+    1556             : 
+    1557             :   /* preconditions //{ */
+    1558             : 
+    1559             :   {
+    1560           2 :     std::stringstream ss;
+    1561             : 
+    1562           2 :     if (!sh_odometry_.hasMsg()) {
+    1563           0 :       ss << "can not land, missing odometry!";
+    1564           0 :       res.message = ss.str();
+    1565           0 :       res.success = false;
+    1566           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1567           0 :       return true;
+    1568             :     }
+    1569             : 
+    1570             :     {
+    1571           2 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1572           0 :         ss << "can not land, missing control manager diagnostics!";
+    1573           0 :         res.message = ss.str();
+    1574           0 :         res.success = false;
+    1575           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1576           0 :         return true;
+    1577             :       }
+    1578             : 
+    1579           2 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1580           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1581           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1582           0 :         res.message = ss.str();
+    1583           0 :         res.success = false;
+    1584           0 :         return true;
+    1585             :       }
+    1586             :     }
+    1587             : 
+    1588           2 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1589           0 :       ss << "can not land, missing controller diagnostics!";
+    1590           0 :       res.message = ss.str();
+    1591           0 :       res.success = false;
+    1592           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1593           0 :       return true;
+    1594             :     }
+    1595             : 
+    1596           2 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1597           0 :       ss << "can not land, missing position cmd!";
+    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             : 
+    1605             :   //}
+    1606             : 
+    1607           2 :   auto [success, message] = landWithDescendImpl();
+    1608             : 
+    1609           2 :   res.message = message;
+    1610           2 :   res.success = success;
+    1611             : 
+    1612           2 :   return true;
+    1613             : }
+    1614             : 
+    1615             : //}
+    1616             : 
+    1617             : /* //{ callbackLandHome() */
+    1618             : 
+    1619           1 : bool UavManager::callbackLandHome([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1620             : 
+    1621           1 :   if (!is_initialized_)
+    1622           0 :     return false;
+    1623             : 
+    1624           1 :   ROS_INFO("[UavManager]: land home called by service");
+    1625             : 
+    1626             :   /* preconditions //{ */
+    1627             : 
+    1628             :   {
+    1629           1 :     std::stringstream ss;
+    1630             : 
+    1631           1 :     if (number_of_takeoffs_ == 0) {
+    1632           0 :       ss << "can not land home, did not takeoff before!";
+    1633           0 :       res.message = ss.str();
+    1634           0 :       res.success = false;
+    1635           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1636           0 :       return true;
+    1637             :     }
+    1638             : 
+    1639           1 :     if (!sh_odometry_.hasMsg()) {
+    1640           0 :       ss << "can not land, missing odometry!";
+    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             :     {
+    1648           1 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1649           0 :         ss << "can not land, missing tracker status!";
+    1650           0 :         res.message = ss.str();
+    1651           0 :         res.success = false;
+    1652           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1653           0 :         return true;
+    1654             :       }
+    1655             : 
+    1656           1 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1657           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1658           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1659           0 :         res.message = ss.str();
+    1660           0 :         res.success = false;
+    1661           0 :         return true;
+    1662             :       }
+    1663             :     }
+    1664             : 
+    1665           1 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1666           0 :       ss << "can not land, missing controller diagnostics command!";
+    1667           0 :       res.message = ss.str();
+    1668           0 :       res.success = false;
+    1669           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1670           0 :       return true;
+    1671             :     }
+    1672             : 
+    1673           1 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1674           0 :       ss << "can not land, missing position cmd!";
+    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           1 :     if (fixing_max_height_) {
+    1682           0 :       ss << "can not land, descedning to safe height!";
+    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           1 :     if (current_state_landing_ != IDLE_STATE) {
+    1690           0 :       ss << "can not land, already landing!";
+    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             : 
+    1698             :   //}
+    1699             : 
+    1700           1 :   ungripSrv();
+    1701             : 
+    1702           2 :   mrs_msgs::ReferenceStamped reference_out;
+    1703             : 
+    1704             :   {
+    1705           1 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1706             : 
+    1707             :     // get the current altitude in land_there_reference_.header.frame_id;
+    1708           1 :     geometry_msgs::PoseStamped current_pose;
+    1709           1 :     current_pose.header.stamp     = ros::Time::now();
+    1710           1 :     current_pose.header.frame_id  = _uav_name_ + "/fcu";
+    1711           1 :     current_pose.pose.position.x  = 0;
+    1712           1 :     current_pose.pose.position.y  = 0;
+    1713           1 :     current_pose.pose.position.z  = 0;
+    1714           1 :     current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1715             : 
+    1716           1 :     auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+    1717             : 
+    1718           1 :     if (response) {
+    1719             : 
+    1720           1 :       land_there_reference_.reference.position.z = response.value().pose.position.z;
+    1721           1 :       ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+    1722             : 
+    1723             :     } else {
+    1724             : 
+    1725           0 :       std::stringstream ss;
+    1726           0 :       ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+    1727           0 :       ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1728             : 
+    1729           0 :       res.success = false;
+    1730           0 :       res.message = ss.str();
+    1731           0 :       return true;
+    1732             :     }
+    1733             : 
+    1734           1 :     reference_out.header.frame_id = land_there_reference_.header.frame_id;
+    1735           1 :     reference_out.header.stamp    = ros::Time::now();
+    1736           1 :     reference_out.reference       = land_there_reference_.reference;
+    1737             :   }
+    1738             : 
+    1739           1 :   bool service_success = emergencyReferenceSrv(reference_out);
+    1740             : 
+    1741           1 :   if (service_success) {
+    1742             : 
+    1743           2 :     std::stringstream ss;
+    1744           1 :     ss << "flying home for landing";
+    1745           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1746             : 
+    1747           1 :     res.success = true;
+    1748           1 :     res.message = ss.str();
+    1749             : 
+    1750             :     // stop the eventual takeoff
+    1751           1 :     waiting_for_takeoff_ = false;
+    1752           1 :     takingoff_           = false;
+    1753           1 :     timer_takeoff_.stop();
+    1754             : 
+    1755           1 :     throttle_under_threshold_          = false;
+    1756           1 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1757             : 
+    1758           1 :     changeLandingState(GOTO_STATE);
+    1759             : 
+    1760           1 :     timer_landing_.start();
+    1761             : 
+    1762             :   } else {
+    1763             : 
+    1764           0 :     std::stringstream ss;
+    1765           0 :     ss << "can not fly home for landing";
+    1766           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1767             : 
+    1768           0 :     res.success = false;
+    1769           0 :     res.message = ss.str();
+    1770             :   }
+    1771             : 
+    1772           1 :   return true;
+    1773             : }
+    1774             : 
+    1775             : //}
+    1776             : 
+    1777             : /* //{ callbackLandThere() */
+    1778             : 
+    1779           0 : bool UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    1780             : 
+    1781           0 :   if (!is_initialized_)
+    1782           0 :     return false;
+    1783             : 
+    1784           0 :   ROS_INFO("[UavManager]: land there called by service");
+    1785             : 
+    1786             :   /* preconditions //{ */
+    1787             : 
+    1788             :   {
+    1789           0 :     std::stringstream ss;
+    1790             : 
+    1791           0 :     if (!sh_odometry_.hasMsg()) {
+    1792           0 :       ss << "can not land, missing odometry!";
+    1793           0 :       res.message = ss.str();
+    1794           0 :       res.success = false;
+    1795           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1796           0 :       return true;
+    1797             :     }
+    1798             : 
+    1799             :     {
+    1800           0 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1801           0 :         ss << "can not land, missing tracker status!";
+    1802           0 :         res.message = ss.str();
+    1803           0 :         res.success = false;
+    1804           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1805           0 :         return true;
+    1806             :       }
+    1807             : 
+    1808           0 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1809           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1810           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1811           0 :         res.message = ss.str();
+    1812           0 :         res.success = false;
+    1813           0 :         return true;
+    1814             :       }
+    1815             :     }
+    1816             : 
+    1817           0 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1818           0 :       ss << "can not land, missing controller diagnostics!";
+    1819           0 :       res.message = ss.str();
+    1820           0 :       res.success = false;
+    1821           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1822           0 :       return true;
+    1823             :     }
+    1824             : 
+    1825           0 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1826           0 :       ss << "can not land, missing position cmd!";
+    1827           0 :       res.message = ss.str();
+    1828           0 :       res.success = false;
+    1829           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1830           0 :       return true;
+    1831             :     }
+    1832             : 
+    1833           0 :     if (fixing_max_height_) {
+    1834           0 :       ss << "can not land, descedning to safe height!";
+    1835           0 :       res.message = ss.str();
+    1836           0 :       res.success = false;
+    1837           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1838           0 :       return true;
+    1839             :     }
+    1840             :   }
+    1841             : 
+    1842             :   //}
+    1843             : 
+    1844           0 :   ungripSrv();
+    1845             : 
+    1846           0 :   mrs_msgs::ReferenceStamped reference_out;
+    1847             : 
+    1848             :   {
+    1849           0 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1850             : 
+    1851           0 :     land_there_reference_.header    = req.header;
+    1852           0 :     land_there_reference_.reference = req.reference;
+    1853             : 
+    1854           0 :     reference_out.header.frame_id = land_there_reference_.header.frame_id;
+    1855           0 :     reference_out.header.stamp    = ros::Time::now();
+    1856           0 :     reference_out.reference       = land_there_reference_.reference;
+    1857             :   }
+    1858             : 
+    1859           0 :   bool service_success = emergencyReferenceSrv(reference_out);
+    1860             : 
+    1861           0 :   if (service_success) {
+    1862             : 
+    1863           0 :     std::stringstream ss;
+    1864           0 :     ss << "flying there for landing";
+    1865           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1866             : 
+    1867           0 :     res.success = true;
+    1868           0 :     res.message = ss.str();
+    1869             : 
+    1870             :     // stop the eventual takeoff
+    1871           0 :     waiting_for_takeoff_ = false;
+    1872           0 :     takingoff_           = false;
+    1873           0 :     timer_takeoff_.stop();
+    1874             : 
+    1875           0 :     throttle_under_threshold_          = false;
+    1876           0 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1877             : 
+    1878           0 :     changeLandingState(GOTO_STATE);
+    1879             : 
+    1880           0 :     timer_landing_.start();
+    1881             : 
+    1882             :   } else {
+    1883             : 
+    1884           0 :     std::stringstream ss;
+    1885           0 :     ss << "can not fly there for landing";
+    1886           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1887             : 
+    1888           0 :     res.success = false;
+    1889           0 :     res.message = ss.str();
+    1890             :   }
+    1891             : 
+    1892           0 :   return true;
+    1893             : }
+    1894             : 
+    1895             : //}
+    1896             : 
+    1897             : /* //{ callbackMidairActivation() */
+    1898             : 
+    1899          44 : bool UavManager::callbackMidairActivation([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1900             : 
+    1901          44 :   if (!is_initialized_)
+    1902           0 :     return false;
+    1903             : 
+    1904          44 :   ROS_INFO("[UavManager]: midair activation called by service");
+    1905             : 
+    1906             :   /* preconditions //{ */
+    1907             : 
+    1908             :   {
+    1909          44 :     std::stringstream ss;
+    1910             : 
+    1911          44 :     if (!sh_odometry_.hasMsg()) {
+    1912           0 :       ss << "can not activate, missing odometry!";
+    1913           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1914           0 :       res.message = ss.str();
+    1915           0 :       res.success = false;
+    1916           0 :       return true;
+    1917             :     }
+    1918             : 
+    1919          44 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1920           0 :       ss << "can not activate, missing HW API status!";
+    1921           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1922           0 :       res.message = ss.str();
+    1923           0 :       res.success = false;
+    1924           0 :       return true;
+    1925             :     }
+    1926             : 
+    1927          44 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1928           0 :       ss << "can not activate, UAV not armed!";
+    1929           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1930           0 :       res.message = ss.str();
+    1931           0 :       res.success = false;
+    1932           0 :       return true;
+    1933             :     }
+    1934             : 
+    1935          44 :     if (sh_hw_api_status_.getMsg()->offboard) {
+    1936           0 :       ss << "can not activate, UAV already in offboard mode!";
+    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             :     {
+    1944          44 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1945           0 :         ss << "can not activate, missing control manager diagnostics!";
+    1946           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1947           0 :         res.message = ss.str();
+    1948           0 :         res.success = false;
+    1949           0 :         return true;
+    1950             :       }
+    1951             : 
+    1952          44 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1953           0 :         ss << "can not activate, need '" << _null_tracker_name_ << "' to be active!";
+    1954           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1955           0 :         res.message = ss.str();
+    1956           0 :         res.success = false;
+    1957           0 :         return true;
+    1958             :       }
+    1959             :     }
+    1960             : 
+    1961          44 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1962           0 :       ss << "can not activate, missing controller diagnostics!";
+    1963           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1964           0 :       res.message = ss.str();
+    1965           0 :       res.success = false;
+    1966           0 :       return true;
+    1967             :     }
+    1968             : 
+    1969          44 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1970           0 :       ss << "can not activate, GainManager is not running!";
+    1971           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1972           0 :       res.message = ss.str();
+    1973           0 :       res.success = false;
+    1974           0 :       return true;
+    1975             :     }
+    1976             : 
+    1977          44 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    1978           0 :       ss << "can not activate, ConstraintManager is not running!";
+    1979           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1980           0 :       res.message = ss.str();
+    1981           0 :       res.success = false;
+    1982           0 :       return true;
+    1983             :     }
+    1984             : 
+    1985          44 :     if (number_of_takeoffs_ > 0) {
+    1986           0 :       ss << "can not activate, we flew already!";
+    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             : 
+    1994             :   //}
+    1995             : 
+    1996          44 :   auto [success, message] = midairActivationImpl();
+    1997             : 
+    1998          44 :   res.message = message;
+    1999          44 :   res.success = success;
+    2000             : 
+    2001          44 :   return true;
+    2002             : }
+    2003             : 
+    2004             : //}
+    2005             : 
+    2006             : // | ------------------------ routines ------------------------ |
+    2007             : 
+    2008             : /* landImpl() //{ */
+    2009             : 
+    2010           3 : std::tuple<bool, std::string> UavManager::landImpl(void) {
+    2011             : 
+    2012             :   // activating the landing controller
+    2013             :   {
+    2014           3 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    2015           3 :     bool        controller_switched = switchControllerSrv(_landing_controller_name_);
+    2016             : 
+    2017             :     // if it fails, activate eland
+    2018             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2019             :     // just throw out error.
+    2020           3 :     if (!controller_switched) {
+    2021             : 
+    2022           0 :       std::stringstream ss;
+    2023           0 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for landing";
+    2024           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2025             : 
+    2026           0 :       elandSrv();
+    2027             : 
+    2028           0 :       return std::tuple(false, ss.str());
+    2029             :     }
+    2030             :   }
+    2031             : 
+    2032             :   // activate the landing tracker
+    2033             :   {
+    2034           3 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    2035           3 :     bool        tracker_switched = switchTrackerSrv(_landing_tracker_name_);
+    2036             : 
+    2037             :     // if it fails, activate eland
+    2038             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2039             :     // just throw out error.
+    2040           3 :     if (!tracker_switched) {
+    2041             : 
+    2042           0 :       std::stringstream ss;
+    2043           0 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for landing";
+    2044           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2045             : 
+    2046           0 :       elandSrv();
+    2047             : 
+    2048           0 :       return std::tuple(false, ss.str());
+    2049             :     }
+    2050             :   }
+    2051             : 
+    2052             :   // call the landing service
+    2053             :   {
+    2054           3 :     bool land_successful = landSrv();
+    2055             : 
+    2056           3 :     if (land_successful) {
+    2057             : 
+    2058             :       // stop the eventual takeoff
+    2059           3 :       waiting_for_takeoff_ = false;
+    2060           3 :       takingoff_           = false;
+    2061           3 :       timer_takeoff_.stop();
+    2062             : 
+    2063             :       // stop counting the flight time
+    2064           3 :       timer_flighttime_.stop();
+    2065             : 
+    2066           6 :       auto controller_diagnostics = sh_controller_diagnostics_.getMsg();
+    2067             : 
+    2068             :       // remember the last valid mass estimated
+    2069             :       // used during subsequent takeoff
+    2070           3 :       if (controller_diagnostics->mass_estimator) {
+    2071           3 :         last_mass_difference_ = controller_diagnostics->mass_difference;
+    2072             :       }
+    2073             : 
+    2074           3 :       setOdometryCallbacksSrv(false);
+    2075             : 
+    2076           3 :       changeLandingState(LANDING_STATE);
+    2077             : 
+    2078           3 :       throttle_under_threshold_          = false;
+    2079           3 :       throttle_mass_estimate_first_time_ = ros::Time(0);
+    2080             : 
+    2081           3 :       timer_landing_.start();
+    2082             : 
+    2083           3 :       std::stringstream ss;
+    2084           3 :       ss << "landing initiated";
+    2085           3 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2086             : 
+    2087           3 :       return std::tuple(true, ss.str());
+    2088             : 
+    2089             :     } else {
+    2090             : 
+    2091           0 :       std::stringstream ss;
+    2092           0 :       ss << "could not land";
+    2093           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2094             : 
+    2095           0 :       elandSrv();
+    2096             : 
+    2097           0 :       return std::tuple(false, ss.str());
+    2098             :     }
+    2099             :   }
+    2100             : }
+    2101             : 
+    2102             : //}
+    2103             : 
+    2104             : /* landWithDescendImpl() //{ */
+    2105             : 
+    2106           5 : std::tuple<bool, std::string> UavManager::landWithDescendImpl(void) {
+    2107             : 
+    2108             :   // if the height information is available
+    2109           5 :   if (sh_height_.hasMsg()) {
+    2110             : 
+    2111           5 :     double height = sh_height_.getMsg()->value;
+    2112             : 
+    2113           5 :     if (height > 0 && height >= _landing_descend_height_ + 1.0) {
+    2114             : 
+    2115           2 :       auto odometry = sh_odometry_.getMsg();
+    2116             : 
+    2117           2 :       ungripSrv();
+    2118             : 
+    2119             :       {
+    2120           2 :         std::scoped_lock lock(mutex_land_there_reference_);
+    2121             : 
+    2122             :         // FOR FUTURE ME: Do not change this, we need it to be filled for the final check later
+    2123           2 :         land_there_reference_.header.frame_id      = "";
+    2124           2 :         land_there_reference_.header.stamp         = ros::Time::now();
+    2125           2 :         land_there_reference_.reference.position.x = odometry->pose.pose.position.x;
+    2126           2 :         land_there_reference_.reference.position.y = odometry->pose.pose.position.y;
+    2127           2 :         land_there_reference_.reference.position.z = odometry->pose.pose.position.z - (height - _landing_descend_height_);
+    2128           2 :         land_there_reference_.reference.heading    = mrs_lib::AttitudeConverter(odometry->pose.pose.orientation).getHeading();
+    2129             :       }
+    2130             : 
+    2131           2 :       bool service_success = emergencyReferenceSrv(land_there_reference_);
+    2132             : 
+    2133           2 :       if (service_success) {
+    2134             : 
+    2135           2 :         std::stringstream ss;
+    2136           2 :         ss << "flying down for landing";
+    2137           2 :         ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2138             : 
+    2139             :         // stop the eventual takeoff
+    2140           2 :         waiting_for_takeoff_ = false;
+    2141           2 :         takingoff_           = false;
+    2142           2 :         timer_takeoff_.stop();
+    2143             : 
+    2144           2 :         changeLandingState(GOTO_STATE);
+    2145             : 
+    2146           2 :         throttle_under_threshold_          = false;
+    2147           2 :         throttle_mass_estimate_first_time_ = ros::Time(0);
+    2148             : 
+    2149           2 :         timer_landing_.start();
+    2150             : 
+    2151           2 :         return std::tuple(true, ss.str());
+    2152             : 
+    2153             :       } else {
+    2154             : 
+    2155           0 :         std::stringstream ss;
+    2156           0 :         ss << "can not fly down for landing";
+    2157           0 :         ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    2158             :       }
+    2159             :     }
+    2160             :   }
+    2161             : 
+    2162           6 :   auto [success, message] = landImpl();
+    2163             : 
+    2164           3 :   return std::tuple(success, message);
+    2165             : }
+    2166             : 
+    2167             : //}
+    2168             : 
+    2169             : /* midairActivationImpl() //{ */
+    2170             : 
+    2171          44 : std::tuple<bool, std::string> UavManager::midairActivationImpl(void) {
+    2172             : 
+    2173             :   // 1. activate the mid-air activation controller
+    2174             :   // the controller will output hover-like control output
+    2175          88 :   std::string old_controller;
+    2176             :   {
+    2177          44 :     old_controller           = sh_control_manager_diag_.getMsg()->active_controller;
+    2178          44 :     bool controller_switched = switchControllerSrv(_midair_activation_during_controller_);
+    2179             : 
+    2180          44 :     if (!controller_switched) {
+    2181             : 
+    2182           0 :       std::stringstream ss;
+    2183           0 :       ss << "could not activate '" << _midair_activation_during_controller_ << "' for midair activation";
+    2184           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2185             : 
+    2186           0 :       return std::tuple(false, ss.str());
+    2187             :     }
+    2188             :   }
+    2189             : 
+    2190             :   // 2. turn Control Manager's output ON
+    2191             :   {
+    2192          44 :     bool output_enabled = toggleControlOutput(true);
+    2193             : 
+    2194          44 :     if (!output_enabled) {
+    2195             : 
+    2196           0 :       switchControllerSrv(old_controller);
+    2197             : 
+    2198           0 :       std::stringstream ss;
+    2199           0 :       ss << "could not enable Control Manager's output";
+    2200           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2201             : 
+    2202           0 :       return std::tuple(false, ss.str());
+    2203             :     }
+    2204             :   }
+    2205             : 
+    2206             :   // 3. activate the mid-air activation tracker
+    2207             :   // this will cause the Control Manager to output something else than min-throttle
+    2208          88 :   std::string old_tracker;
+    2209             :   {
+    2210          44 :     old_tracker = sh_control_manager_diag_.getMsg()->active_tracker;
+    2211             : 
+    2212          44 :     bool tracker_switched = switchTrackerSrv(_midair_activation_during_tracker_);
+    2213             : 
+    2214          44 :     if (!tracker_switched) {
+    2215             : 
+    2216           0 :       switchControllerSrv(old_controller);
+    2217           0 :       toggleControlOutput(false);
+    2218             : 
+    2219           0 :       std::stringstream ss;
+    2220           0 :       ss << "could not activate '" << _midair_activation_during_tracker_ << "' for midair activation";
+    2221           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2222             : 
+    2223           0 :       return std::tuple(false, ss.str());
+    2224             :     }
+    2225             :   }
+    2226             : 
+    2227             :   // 4. wait for 50 ms, that should be enough for the Pixhawk to start getting data
+    2228          44 :   ros::Duration(0.05).sleep();
+    2229             : 
+    2230             :   // 5. turn on the OFFBOARD MODE
+    2231             :   // since now, the UAV should be under our control
+    2232             :   {
+    2233          44 :     bool offboard_set = offboardSrv(true);
+    2234             : 
+    2235          44 :     if (!offboard_set) {
+    2236             : 
+    2237           0 :       switchTrackerSrv(old_tracker);
+    2238           0 :       switchControllerSrv(old_controller);
+    2239           0 :       toggleControlOutput(false);
+    2240             : 
+    2241           0 :       std::stringstream ss;
+    2242           0 :       ss << "could not activate offboard mode";
+    2243           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2244             : 
+    2245           0 :       return std::tuple(false, ss.str());
+    2246             :     }
+    2247             :   }
+    2248             : 
+    2249             :   // remember this time, later check for timeout
+    2250          44 :   midair_activation_started_ = ros::Time::now();
+    2251             : 
+    2252             :   // start the timer which should check if the offboard is on, activate proper controller and tracker or timeout
+    2253          44 :   timer_midair_activation_.start();
+    2254             : 
+    2255          44 :   std::stringstream ss;
+    2256          44 :   ss << "midair activation initiated, starting the timer";
+    2257          44 :   ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2258             : 
+    2259          44 :   return std::tuple(true, ss.str());
+    2260             : }
+    2261             : 
+    2262             : //}
+    2263             : 
+    2264             : // | ----------------- service client wrappers ---------------- |
+    2265             : 
+    2266             : /* setOdometryCallbacksSrv() //{ */
+    2267             : 
+    2268          67 : void UavManager::setOdometryCallbacksSrv(const bool& input) {
+    2269             : 
+    2270          67 :   ROS_INFO("[UavManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    2271             : 
+    2272         134 :   std_srvs::SetBool srv;
+    2273             : 
+    2274          67 :   srv.request.data = input;
+    2275             : 
+    2276          67 :   bool res = sch_odometry_callbacks_.call(srv);
+    2277             : 
+    2278          67 :   if (res) {
+    2279             : 
+    2280          67 :     if (!srv.response.success) {
+    2281           0 :       ROS_WARN("[UavManager]: service call for toggle odometry callbacks returned: %s.", srv.response.message.c_str());
+    2282             :     }
+    2283             : 
+    2284             :   } else {
+    2285           0 :     ROS_ERROR("[UavManager]: service call for toggle odometry callbacks failed!");
+    2286             :   }
+    2287          67 : }
+    2288             : 
+    2289             : //}
+    2290             : 
+    2291             : /* setControlCallbacksSrv() //{ */
+    2292             : 
+    2293          84 : void UavManager::setControlCallbacksSrv(const bool& input) {
+    2294             : 
+    2295          84 :   ROS_INFO("[UavManager]: switching control callbacks to %s", input ? "ON" : "OFF");
+    2296             : 
+    2297         168 :   std_srvs::SetBool srv;
+    2298             : 
+    2299          84 :   srv.request.data = input;
+    2300             : 
+    2301          84 :   bool res = sch_control_callbacks_.call(srv);
+    2302             : 
+    2303          84 :   if (res) {
+    2304             : 
+    2305          84 :     if (!srv.response.success) {
+    2306           0 :       ROS_WARN("[UavManager]: service call for setting control callbacks returned: %s.", srv.response.message.c_str());
+    2307             :     }
+    2308             : 
+    2309             :   } else {
+    2310           0 :     ROS_ERROR("[UavManager]: service call for setting control callbacks failed!");
+    2311             :   }
+    2312          84 : }
+    2313             : 
+    2314             : //}
+    2315             : 
+    2316             : /* ungripSrv() //{ */
+    2317             : 
+    2318           3 : void UavManager::ungripSrv(void) {
+    2319             : 
+    2320           3 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: ungripping payload");
+    2321             : 
+    2322           6 :   std_srvs::Trigger srv;
+    2323             : 
+    2324           3 :   bool res = sch_ungrip_.call(srv);
+    2325             : 
+    2326           3 :   if (res) {
+    2327             : 
+    2328           0 :     if (!srv.response.success) {
+    2329           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload returned: %s.", srv.response.message.c_str());
+    2330             :     }
+    2331             : 
+    2332             :   } else {
+    2333           3 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload failed!");
+    2334             :   }
+    2335           3 : }
+    2336             : 
+    2337             : //}
+    2338             : 
+    2339             : /* toggleControlOutput() //{ */
+    2340             : 
+    2341          44 : bool UavManager::toggleControlOutput(const bool& input) {
+    2342             : 
+    2343          44 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: toggling control output %s", input ? "ON" : "OFF");
+    2344             : 
+    2345          88 :   std_srvs::SetBool srv;
+    2346             : 
+    2347          44 :   srv.request.data = input;
+    2348             : 
+    2349          44 :   bool res = sch_toggle_control_output_.call(srv);
+    2350             : 
+    2351          44 :   if (res) {
+    2352             : 
+    2353          44 :     if (!srv.response.success) {
+    2354           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output returned: %s.", srv.response.message.c_str());
+    2355           0 :       return false;
+    2356             :     } else {
+    2357          44 :       return true;
+    2358             :     }
+    2359             : 
+    2360             :   } else {
+    2361           0 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output failed!");
+    2362           0 :     return false;
+    2363             :   }
+    2364             : }
+    2365             : 
+    2366             : //}
+    2367             : 
+    2368             : /* offboardSrv() //{ */
+    2369             : 
+    2370          44 : bool UavManager::offboardSrv(const bool in) {
+    2371             : 
+    2372          44 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: setting offboard to %d", in);
+    2373             : 
+    2374          88 :   std_srvs::Trigger srv;
+    2375             : 
+    2376          44 :   bool res = sch_offboard_.call(srv);
+    2377             : 
+    2378          44 :   if (!res) {
+    2379             : 
+    2380           0 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed!");
+    2381           0 :     return false;
+    2382             : 
+    2383             :   } else {
+    2384             : 
+    2385          44 :     if (!srv.response.success) {
+    2386           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed, returned: %s", srv.response.message.c_str());
+    2387           0 :       return false;
+    2388             :     } else {
+    2389          44 :       return true;
+    2390             :     }
+    2391             :   }
+    2392             : }
+    2393             : 
+    2394             : //}
+    2395             : 
+    2396             : /* pirouetteSrv() //{ */
+    2397             : 
+    2398           0 : void UavManager::pirouetteSrv(void) {
+    2399             : 
+    2400           0 :   std_srvs::Trigger srv;
+    2401             : 
+    2402           0 :   bool res = sch_pirouette_.call(srv);
+    2403             : 
+    2404           0 :   if (res) {
+    2405             : 
+    2406           0 :     if (!srv.response.success) {
+    2407           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call for pirouette returned: %s.", srv.response.message.c_str());
+    2408             :     }
+    2409             : 
+    2410             :   } else {
+    2411           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for pirouette failed!");
+    2412             :   }
+    2413           0 : }
+    2414             : 
+    2415             : //}
+    2416             : 
+    2417             : /* disarmSrv() //{ */
+    2418             : 
+    2419           3 : void UavManager::disarmSrv(void) {
+    2420             : 
+    2421           6 :   std_srvs::SetBool srv;
+    2422             : 
+    2423           3 :   srv.request.data = false;
+    2424             : 
+    2425           3 :   bool res = sch_arm_.call(srv);
+    2426             : 
+    2427           3 :   if (res) {
+    2428             : 
+    2429           3 :     if (!srv.response.success) {
+    2430           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call disarming returned: %s.", srv.response.message.c_str());
+    2431             :     }
+    2432             : 
+    2433             :   } else {
+    2434           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for disarming failed!");
+    2435             :   }
+    2436           3 : }
+    2437             : 
+    2438             : //}
+    2439             : 
+    2440             : /* switchControllerSrv() //{ */
+    2441             : 
+    2442         111 : bool UavManager::switchControllerSrv(const std::string& controller) {
+    2443             : 
+    2444         111 :   ROS_INFO_STREAM("[UavManager]: activating controller '" << controller << "'");
+    2445             : 
+    2446         222 :   mrs_msgs::String srv;
+    2447         111 :   srv.request.value = controller;
+    2448             : 
+    2449         111 :   bool res = sch_switch_controller_.call(srv);
+    2450             : 
+    2451         111 :   if (res) {
+    2452             : 
+    2453         111 :     if (!srv.response.success) {
+    2454           0 :       ROS_WARN("[UavManager]: service call for switching controller returned: '%s'", srv.response.message.c_str());
+    2455             :     }
+    2456             : 
+    2457         111 :     return srv.response.success;
+    2458             : 
+    2459             :   } else {
+    2460             : 
+    2461           0 :     ROS_ERROR("[UavManager]: service call for switching controller failed!");
+    2462             : 
+    2463           0 :     return false;
+    2464             :   }
+    2465             : }
+    2466             : 
+    2467             : //}
+    2468             : 
+    2469             : /* switchTrackerSrv() //{ */
+    2470             : 
+    2471         114 : bool UavManager::switchTrackerSrv(const std::string& tracker) {
+    2472             : 
+    2473         114 :   ROS_INFO_STREAM("[UavManager]: activating tracker '" << tracker << "'");
+    2474             : 
+    2475             : 
+    2476         228 :   mrs_msgs::String srv;
+    2477         114 :   srv.request.value = tracker;
+    2478             : 
+    2479         114 :   bool res = sch_switch_tracker_.call(srv);
+    2480             : 
+    2481         114 :   if (res) {
+    2482             : 
+    2483         114 :     if (!srv.response.success) {
+    2484           0 :       ROS_WARN("[UavManager]: service call for switching tracker returned: '%s'", srv.response.message.c_str());
+    2485             :     }
+    2486             : 
+    2487         114 :     return srv.response.success;
+    2488             : 
+    2489             :   } else {
+    2490             : 
+    2491           0 :     ROS_ERROR("[UavManager]: service call for switching tracker failed!");
+    2492             : 
+    2493           0 :     return false;
+    2494             :   }
+    2495             : }
+    2496             : 
+    2497             : //}
+    2498             : 
+    2499             : /* landSrv() //{ */
+    2500             : 
+    2501           3 : bool UavManager::landSrv(void) {
+    2502             : 
+    2503           3 :   ROS_INFO("[UavManager]: calling for landing");
+    2504             : 
+    2505           6 :   std_srvs::Trigger srv;
+    2506             : 
+    2507           3 :   bool res = sch_land_.call(srv);
+    2508             : 
+    2509           3 :   if (res) {
+    2510             : 
+    2511           3 :     if (!srv.response.success) {
+    2512           0 :       ROS_WARN("[UavManager]: service call for landing returned: '%s'", srv.response.message.c_str());
+    2513             :     }
+    2514             : 
+    2515           3 :     return srv.response.success;
+    2516             : 
+    2517             :   } else {
+    2518             : 
+    2519           0 :     ROS_ERROR("[UavManager]: service call for landing failed!");
+    2520             : 
+    2521           0 :     return false;
+    2522             :   }
+    2523             : }
+    2524             : 
+    2525             : //}
+    2526             : 
+    2527             : /* elandSrv() //{ */
+    2528             : 
+    2529           0 : bool UavManager::elandSrv(void) {
+    2530             : 
+    2531           0 :   ROS_INFO("[UavManager]: calling for eland");
+    2532             : 
+    2533           0 :   std_srvs::Trigger srv;
+    2534             : 
+    2535           0 :   bool res = sch_eland_.call(srv);
+    2536             : 
+    2537           0 :   if (res) {
+    2538             : 
+    2539           0 :     if (!srv.response.success) {
+    2540           0 :       ROS_WARN("[UavManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    2541             :     }
+    2542             : 
+    2543           0 :     return srv.response.success;
+    2544             : 
+    2545             :   } else {
+    2546             : 
+    2547           0 :     ROS_ERROR("[UavManager]: service call for eland failed!");
+    2548             : 
+    2549           0 :     return false;
+    2550             :   }
+    2551             : }
+    2552             : 
+    2553             : //}
+    2554             : 
+    2555             : /* ehoverSrv() //{ */
+    2556             : 
+    2557           0 : bool UavManager::ehoverSrv(void) {
+    2558             : 
+    2559           0 :   ROS_INFO("[UavManager]: calling for ehover");
+    2560             : 
+    2561           0 :   std_srvs::Trigger srv;
+    2562             : 
+    2563           0 :   bool res = sch_ehover_.call(srv);
+    2564             : 
+    2565           0 :   if (res) {
+    2566             : 
+    2567           0 :     if (!srv.response.success) {
+    2568           0 :       ROS_WARN("[UavManager]: service call for ehover returned: '%s'", srv.response.message.c_str());
+    2569             :     }
+    2570             : 
+    2571           0 :     return srv.response.success;
+    2572             : 
+    2573             :   } else {
+    2574             : 
+    2575           0 :     ROS_ERROR("[UavManager]: service call for ehover failed!");
+    2576             : 
+    2577           0 :     return false;
+    2578             :   }
+    2579             : }
+    2580             : 
+    2581             : //}
+    2582             : 
+    2583             : /* takeoffSrv() //{ */
+    2584             : 
+    2585          10 : bool UavManager::takeoffSrv(void) {
+    2586             : 
+    2587          10 :   ROS_INFO("[UavManager]: calling for takeoff to height '%.2f m'", _takeoff_height_);
+    2588             : 
+    2589          20 :   mrs_msgs::Vec1 srv;
+    2590             : 
+    2591          10 :   srv.request.goal = _takeoff_height_;
+    2592             : 
+    2593          10 :   bool res = sch_takeoff_.call(srv);
+    2594             : 
+    2595          10 :   if (res) {
+    2596             : 
+    2597          10 :     if (!srv.response.success) {
+    2598           0 :       ROS_WARN("[UavManager]: service call for takeoff returned: '%s'", srv.response.message.c_str());
+    2599             :     }
+    2600             : 
+    2601          10 :     return srv.response.success;
+    2602             : 
+    2603             :   } else {
+    2604             : 
+    2605           0 :     ROS_ERROR("[UavManager]: service call for takeoff failed!");
+    2606             : 
+    2607           0 :     return false;
+    2608             :   }
+    2609             : }
+    2610             : 
+    2611             : //}
+    2612             : 
+    2613             : /* emergencyReferenceSrv() //{ */
+    2614             : 
+    2615          82 : bool UavManager::emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal) {
+    2616             : 
+    2617          82 :   ROS_INFO_THROTTLE(1.0, "[UavManager]: calling for emergency reference");
+    2618             : 
+    2619         164 :   mrs_msgs::ReferenceStampedSrv srv;
+    2620             : 
+    2621          82 :   srv.request.header    = goal.header;
+    2622          82 :   srv.request.reference = goal.reference;
+    2623             : 
+    2624          82 :   bool res = sch_emergency_reference_.call(srv);
+    2625             : 
+    2626          82 :   if (res) {
+    2627             : 
+    2628          82 :     if (!srv.response.success) {
+    2629           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call for emergency reference returned: '%s'", srv.response.message.c_str());
+    2630             :     }
+    2631             : 
+    2632          82 :     return srv.response.success;
+    2633             : 
+    2634             :   } else {
+    2635             : 
+    2636           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for emergency reference failed!");
+    2637             : 
+    2638           0 :     return false;
+    2639             :   }
+    2640             : }
+    2641             : 
+    2642             : //}
+    2643             : 
+    2644             : }  // namespace uav_manager
+    2645             : 
+    2646             : }  // namespace mrs_uav_managers
+    2647             : 
+    2648             : #include <pluginlib/class_list_macros.h>
+    2649          57 : 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..8be46f90cc --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html @@ -0,0 +1,683 @@ + + + + + + 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 + + +
+ 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..eff382234d385de85aebe8a2bb350645b02183f9 GIT binary patch literal 7236 zcmV-K9J}L*P)XMBrt!Vqoz5^Q2#A1sxenLsb^QJB zV%2`}tw^selnGD(tnB*LMsq-muEj0KXn_}D6atDd%Da}2kq7Ps6wh@9l$se5UOa^i z#*!u)P>PZ2yBko)9%0uT6{oS>hpq=F zk?RPoFs}I8fGg1gjP_Jt#PX*Dv~h!76Um~gt^5vXhrtM^Hv-$OiN`lygQ#D9PGPc-)T+9Z{R$DQ1d{pz_E`Q4WL4LN_osk87aUq2OGK+&YQ3H?~=n~jT8cuW28_roDl;u1C_MMp0+zf z#;&~m2V8l4_2WYkD`=9o$Y^o@}ZgoqPo0B{gSOByX>o*1wdIoAWqyKW@9I>so**qk3h zi}AH4AfN<@P*rfnfOW_&z@!xf?`dz3Kq&?kw<}mc>wvY^6&i3`;HIeuArjExK-(T_ z^`sq549Injc+K=IZwH|o0`lbU!k#gR6urQ}F+zfQz)KFuMIduRDty!sOXt;)0-e(=o54;imt4^wW1yl@RDs&S(RD83tX;& zOM2t3Yk-B%-Wa3F{9P`pq-J5x*pi{r?nwYSZ6GjmfDzn?LmDwNC#DP;a11X1Nt_t4 zzFy~i3NreQ`-X~_xp4Y zZJuvzyW9d4590tUNDxLT;7)vW>Sk2E3;F2Uw2|uD?|j^;He*0JM%ZeAeHibK2`pe7 z_{{c>#4IwP5URXu$SD?Z_81ufExT?4dW@XJXJZ7!6f@b!a7H}V);on$A;l5nf%MwY zLgsqSTgc8SM1rsx$t|9(j?t+g?hfIEzlZK!J&ZbpE@_QE)O z=B$4g3xgVC-1rPBSP+Vb8&pq}OtiJI0w}^*Td!4A4?aHP3!n$UgE54wSWQ6FNdp3p zyUrdGP)ZEJ$Em=3x5r3QUHBC%rzC(YZV})t#yATBCke*z;SGEY;wB(wk^(wl8}=ll zU8gz&7LHLxjKR$8I>vnft*8!y)&`vZ&ePpKFiM0N|tAE{|s&AOgx;c0z0jC-$~aA#Llxip}`AB_l~ddS;|kXn!v5KLsi& zF*^g5thC*e0r0=q1{7oTcqM?dtZV5QZwg|BUEekAkpa$9jN-1_7^Tuf8GyVJqRRmA zL-S3uS3ss2{IM`nj+dn-;ysri4F;r~;hDxr?5`O>a@)*gAwGbQzJr$FMx$t_*RvEb z01K^|+&9}G*kL1`zP?Y zdyM%WWfLD){OnZEcRgT3TcJFP0q@m?NzrxLKo9xm%o?L~u5Z~^mhO9+ON`7QXO+4} z0c%JPo9Mb*Y{Y|ro9t(1-uw3qmyVH2OWR?5@ojR+yL*x6Iz(l`4z&u#s|UEgXGLW& z#4_h()O zI7!nnjD1ltoVRpkQ|?L>#pVD|h_P>sCIxS_v5$H?1tZsC?2{cJT|T%Qu7;7eWdSIi z%qxac_ChAD3@`Dol!bjy-o3rcgsm@ln?Hu#a;Va7%iFrc1wfot$11 zvMvTxt3uj|Lu?da560>#wp)Ft4!ibpz>u2&g`2aNP&|K__WW6(P-H)ey5s&@!uLF2 zTh4v~WlE#a8OpzQ@)2jkB^GZ*G&R&pWSrH7NL&vS#!>*vF}k>9eJV8ufSE&Y?);zS z1t^*t&CFm^QO=UCS}~_(2_$(U)}pEPU%55QF(#Z8pbXT>v+>^$yeUL=Gi)*iiYuAfoPHnfK-7Dmrp)e~k5}sjVKMUyk zeF2sjB@s=PnDKM?bYDL|Xyfw}G=gFH4%?JWCZ6ddP|WRHQW~%mBguqrRbcgNgDQB* z@;mE|7~6om>I}#6{wWNRNsPB2V---UGv?`d<293ukwT8HOOj$EE~R;gny<6^p$7PO zs~_l&)kR-0y>Agqp_5JbYZL1@7^fWQEIx4;vao>=*AT9Cq}3bOhx5yaA3k^GBnB*IxxCg5t^_3V^*&ht^o9y&!DY71Xk?s?K()q;HCfU^mR zMUe$$Z>sMeBak<9z(m!oD;OvfaWvT*F!F#3jG06-$n{&&met*`B`pPK1ID6~p$p^Q zF;Za8mb~z*zmAkMfu9f}S!X6Ipzp1d64f8{V@yRU6RsHO!dMBfX%@|M+nia8LvNb{ z0TZZ7*>;Te-5YUsAAWAc&y6^Z@ogI)DaMQR4w`d^)DSc1kMdrm=?>|BCvMRaVmx<9 zkt%&%Gs1W7kUB9EtEL&oqwbJ^*%KP6$B}nPo6a_8O1OEh&mEExMsP))&C8qR4hcBd z_r5FYzPS*HJb(m3RNBqi`l1}fMF8qB-fIg22BZr6d%E^#)8ZrH11J_BVG>Pps!7*% ztcZox^F(Q^epOhOy7c1ZqjB<6({)(l+*#37x!)fIY$@VqKsm-ZB6PU8H)dZP@c98z3}nr8W-nT1 ze7J`O^vg&aV|LF+tQOFo$oYSD_Dd?aclSx9me$N4+}1H#4)E2ii7xgt+=gNA*MW0| z$dpZ`3X&Pj#4MXLJd1K-B*%7~D(rm8m^mLSG-*rQC;5>#-RXpuB5h=+n(RY0gobbi~}CCls)kC7WoS;hd_DcsLPM$ghq` z7~6nSjC*^{?33eKubIt_V8B*5qbCDt7w>V`-VW)twt=yjSeq?~$@W~Ovgk3!qfPZA zPMrye%G($HXM84xnF+A=;)cF!=7!w(JvBy2t@Mg9l2ExAI0LNRAp8Q9K0oocLq3^= zSD5s639r_XxMm|}P}p`BGh(frX~2@^4DbMAM(ap1#-jx)ZSqZodmXt2J?&$;*HMk) zvlpd=Yl#7j*|yH)EDj?z!bqV~V-&05Ew_&*H)+dG+iRi<%$4oWe0$$>SCf_C;xTf# z)fffg2l+{4`v63bnDGxlq1sk5iQ_L{nyTq*Afp4Abx8}Lh>PiF*;!(=psH-b7 zjmZ%&xlg4~WD;hQW0#1n8l!3T>_Nh_)y6#oR~#SS^%=PQ@$_(aVL-U?6;YEKEy`;q zjzvZ4Q2=`jOb)E+*_-*sUKWCAAJnC{xs>mi* zHsr5bq-w=y)!9RU4WV$|wS?k7yaL6%=11}oooUFa{&GGtQd#iPS7-55pM1O%0Sl6k zQ8Fp2zl)EmP!r+X7o|=9I_tucf*UV?BGn>}C%^{B5n!F+B98BL9bxOdG{eUQ+zY;b zSU>+YKJF0Z5~V6w4*)+|_obEsds6iJPkI2r2SD%coqS|jN%E0m<;lmYdaE)OlmS@t z6mT;g56E>~PMm&|ai zpTp~JA~n={QZnC6rd;J1=^q!muB~LeL#F|387ai-I5SykgP!WzeSk#QQossYt-2;7CKL*rclXGx%$E0O z_Ph{8=pe10P%6}AT!qhekS=lkh;k9X&Ww5Nx`px0mt!>)&`HlRO5JvU>C_t+^l3g~ zVcHb)TxcL?VVX0#9N+_{6=ImOQV3+7Y!chYBNnjGbpQm%7eIZYdY}AElj8?0U?GZE zJUQQ*5R)^!r+_8OYgTj>iTOfHA$-)9;UmF;KgP!cC1}^2YUL>pZC1+RG6K|e{Rf@l z9nbi9waq^r(-~%tX`IgR>Qb;bX+MBze*VO=_`?4FG1Dw<=cb|xXs-*m?VF17Sj@Sp zI5!oTj~}_IcmkvGrIzB{R8#@`_*hGRJ}@8G;@Xl|Cm-+0#gyxHgFT1m<1se5cw0cL zLK7ka<`&8;*%pi)dxcuLfVQ3j6nc!*MJr?8HSC|H8V!qozc8o31EO*P8jvPte?VNA z`tq$jpsdA`7^cKEepkQUtJwJfD8^W+R~uVJO&MSnRJp{(!pLeNP@=f@DnS5TKS%(PtZX5sSY4SB5Z4jjCu-5| z?V6vRPK-8lZT^Ii_6XGCqp7TcE{{ObtB3xWmhcFa=up)zN1*;>e#RzY08j3+Z9(sI z)#o~4tWM?*`5AN$aS47D9*d%G?|u0hg#nPVCx6q9K2!Yw9_g7^*fxg`#Bq-AXI??W z$0uvD&b)$ABjuJ6+_P9o!AJ59QI3yiUV-EzyKYf@{EocB0pj5gY!WJblu`X4nY{&L zhi_Jx$wJJ8#314MoqLiVau-Ljw!drg0v`e}46ztx4*2?6fiVD@Z}-4eL<_Nm^{^++ zF=M^e7`3;X_!=of914s9Z(spT?ssnO1-m%W)?V7RIBo{1UMd^O0JwgT03uo0LQFn0 zPj&>QUMB%I>6)LNPK-8l{ULF)kb7xT?PdM%$IWCu$}dJ?&x)>he^}0-wz=}KT=t_Q z);0UI?!BmOKI@uSw8Gox`N)}euVBAT`Gl$_>Fx85kCX#+^06wFB_I7=LoC%N9|dk&bJ0i)Fzqs{b26gFrK&g-ljm)WgkkKn#tf3?}@@5}VpqF%!Fq1@%7km^5mowdy3 z28@O^abZ;e17ylW-?%FL;mfDrr89rvI%|!Df1}PwG^X#;CRX7F4!bs|w*A2Jj>j$%$<7FmZ;1TB8+a!}aQZ zg`1qInKHb;mN%|+9SeKyVnsd1ngvy3yu9z73NPjUiI(7Xjo~e>Aw2Jadql;R^IoCfX&$g=NG2LX1NAT71SQt{G9~T$SUqc>~9ESv1Zb+-rqco&U!aQ}O3l>BV zjz`;Tajmm^O`XY&YegPqV)n2!_RDGP<3r*gh~Yo1YJ?wdH;^9RwczR3kPjN;lc~&m z%=!fhsMaQ8fN~k#j#tWKRm={ig+KHWwC~lz=>HF31uF$NQ$iFX>=Lu|*l2u-I=rBq z{v`~6#Wi{nQ|oTU7&NifNG4jYdN(TCt{<%CE2vHN&+axh_0z;0rOSC z?~vkdDVl=?{Y!qsJ5MBFMyD!n5F>h^RP_wy;XpDrlO`qnYFJp~uBRj7in?QnUZp?!28j$t4F zVhy|9De<4oX?{&Ndkd>ja6!_gb^oIr}G` zF9qI$@k7ksf33zjoY~_m3!bOhtGChL + + + + + + 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-01-07 21:47:14Functions: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()47
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()47
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().247
+
+
+ + + +
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..404c3f3cd7 --- /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-01-07 21:47:14Functions: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()47
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()47
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().247
+
+
+ + + +
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..fc91a6dd1a --- /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-01-07 21:47:14Functions: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          47 :   GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) {
+      57          47 :   }
+      58             : 
+      59          94 :   ~GarminAgl(void) {
+      60          94 :   }
+      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-01-07 21:47:14Functions: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..e5211055a2 --- /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-01-07 21:47:14Functions: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..048ee6ef4e --- /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-01-07 21:47:14Functions: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..b01f4c8d3e --- /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-01-07 21:47:14Functions: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..63d2136543 --- /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-01-07 21:47:14Functions: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..4057b7fa75 --- /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-01-07 21:47:14Functions: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..af808943b6 --- /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-01-07 21:47:14Functions: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)107
mrs_uav_state_estimators::AltGeneric::~AltGeneric()107
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2107
+
+
+ + + +
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..8251e50779 --- /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-01-07 21:47:14Functions: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)107
mrs_uav_state_estimators::AltGeneric::~AltGeneric()107
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2107
+
+
+ + + +
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..e575db82bb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html @@ -0,0 +1,242 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fdd77c42987a7bc3630f5b90c60f6d87d72bbc76 GIT binary patch literal 682 zcmV;b0#*HqP)(mad{$f%qh)u%DK)n18Omi@VH(-O z)_}`B6W2G~-*Anm>(3a#TneMWW6mw0+Yr#5A)jS30HK+ue@s(ebjWF^N?g&FDXkhc zG~zLTeZWj>n!`|lHse2V4KNqve8q6arh4#3f`bu-$|g1;(RmLEU`=*H?^@9!0UI@^ zm|l~j@|VDFvh+i4168u#6@k{4`U+GgvM(ilW+X-t>oXb$SXA0+25Diyh?J6uyE9+$ zkQFRg3M?2zjk7C!Ex;D`{4v0J3_tQ%@`7ugeo`pQl6^9|GAJxAvEYWv%p@X;bXImx zy;K(RwOpYq0C zFMGUT-OP4Py4e!X-YB;BWv~3Xr@wdpyspExZTnlc7ySy`zPT*;Ulg;WflYMLA$uz%m|-ns!A!oyX6teHI$8v@6Ddi^C>a(n?R5I16zQm+97J9 QqyPW_07*qoM6N<$f@O^^S^xk5 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..a27f7c2666 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-01-07 21:47:14Functions: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&)107
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2107
+
+
+ + + +
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..5f36bb51c2 --- /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-01-07 21:47:14Functions: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&)107
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2107
+
+
+ + + +
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..5cd4e42abd --- /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-01-07 21:47:14Functions: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         107 :   AltitudeEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(altitude::type, name, frame_id) {
+      31         107 :   }
+      32             : 
+      33         107 :   virtual ~AltitudeEstimator(void) {
+      34         107 :   }
+      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..b556b8424e --- /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-01-07 21:47:14Functions: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..7912072647 --- /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-01-07 21:47:14Functions: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..94adf828ce --- /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-01-07 21:47:14Functions: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..c1c6964ec0 --- /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-01-07 21:47:14Functions: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..0cf5762d5c --- /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-01-07 21:47:14Functions: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..f1915ea66f --- /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-01-07 21:47:14Functions: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..7b9d32dabb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html @@ -0,0 +1,348 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37865557.7 %
Date:2024-01-07 21:47:14Functions:476770.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getVelInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::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>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)60
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)>)64
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const128
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const131
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)>)163
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)164
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const258
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const326
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const367
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const661
mrs_uav_state_estimators::Correction<2>::getRawCorrection()2513
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()2513
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2973
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2990
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const2990
mrs_uav_state_estimators::Correction<1>::getRawCorrection()4453
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()4453
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5487
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5545
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5547
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10977
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)11000
mrs_uav_state_estimators::Correction<2>::getVelInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)11001
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)97509
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)98858
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)106881
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)106914
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)106921
mrs_uav_state_estimators::Correction<2>::setR(double)113673
mrs_uav_state_estimators::Correction<2>::getStateId() const113825
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)113953
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)115385
mrs_uav_state_estimators::Correction<2>::isHealthy()127407
mrs_uav_state_estimators::Correction<2>::isMsgComing()127408
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)168522
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)170690
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)229017
mrs_uav_state_estimators::Correction<1>::setR(double)275585
mrs_uav_state_estimators::Correction<1>::getStateId() const275586
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)278374
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)281055
mrs_uav_state_estimators::Correction<1>::isHealthy()303237
mrs_uav_state_estimators::Correction<1>::isMsgComing()303256
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)556610
mrs_uav_state_estimators::Correction<2>::getR()571544
mrs_uav_state_estimators::Correction<1>::getR()831972
+
+
+ + + +
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..f5e32d5fd5 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html @@ -0,0 +1,348 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37865557.7 %
Date:2024-01-07 21:47:14Functions:476770.1 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2973
mrs_uav_state_estimators::Correction<1>::isMsgComing()303256
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>)168522
mrs_uav_state_estimators::Correction<1>::getVelInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)106881
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&)278374
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)106914
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()4453
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)556610
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2990
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>)170690
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()4453
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)164
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)106921
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()831972
mrs_uav_state_estimators::Correction<1>::setR(double)275585
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)281055
mrs_uav_state_estimators::Correction<1>::isHealthy()303237
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)>)163
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5487
mrs_uav_state_estimators::Correction<2>::isMsgComing()127408
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)97509
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getVelInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)11001
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10977
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&)113953
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()2513
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)229017
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5547
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>)98858
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()2513
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)60
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)11000
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()571544
mrs_uav_state_estimators::Correction<2>::setR(double)113673
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)115385
mrs_uav_state_estimators::Correction<2>::isHealthy()127407
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)>)64
mrs_uav_state_estimators::Correction<1>::getStateId() const275586
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const367
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const661
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const2990
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const326
mrs_uav_state_estimators::Correction<2>::getStateId() const113825
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const131
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const258
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5545
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const128
+
+
+ + + +
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..a0d3834df4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html @@ -0,0 +1,1795 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37865557.7 %
Date:2024-01-07 21:47:14Functions:476770.1 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..70eb8d6d1e4db1530114d7c7033612d40a642d6c GIT binary patch literal 5131 zcmV+m6!hzfP)kG-2W;&<_ihGSRLE0dGmHUEhRvJ5J+NoyWNJ{t<#$a zOl^1_(A-uuy~p-CvJzu#`vcOPmw>E%2#j>^HJ}9}*KHFrl}GvLOq zcQ6@%aSv=5qoR}o;LQw(fHlB&7DbFLG^V~4zy^#rBjB!FH{;sS^C$H@veSO;unIsp&E4g>rlay0hd()EcLZQNMl2BY#I?b$_nI%NLwW#x>NuFM{5 z1E95Q%?6I7vBgh&`KCdX06kD|$F29B{S@;0xFNv70_fOa0ozDy@DzmVdUffcbZ^BJ z2U~CN4|knRMErJ}%N;B}D6^};nY%W&rjdaIGbH)<;h!F=MNj&h0qSZ3tl=Wq(5Ptn zONuCwc1h3(u-_xVzj+JOF*%Y_zj6`)jKw|+8vk2e)WnaGX>x!`S#Hj)@Jsd#I2ZxD z-7y9q$+K>0X;}I_7lx&+kh13iExJc{ZF7$e2wS2(FfzaLe3gb$JU~c80jPaHC?Lkf zb<{#NO=BL+Ap@Flur$_yJJ5(oeH9}$5ztt1zXI3j%{~Ci1od&IPyn;%pjnS#&S{0W z*u0ykxqVq6j7cAQkAN(W9(+aAo}T+)GmSW$5W0qai_8GPeXQ^Xw5+dViiSQmcRfHr zu1BMI5a5e65>}4^n74bL#!}`ixl|t}(lx+!*0G2Le-)@_3@mOr5boQgO;C-+2Y9yu z4g%m1Y^GV2s~L(zLjf|LkitmF4lEkAt_x;+B`gHOd(pN-hcp8f;1GYwfB^Ap)B%jW zK{o@S$_oJ0G1?3x44clfLzQ%Xiyf0RCVki=rC;h8^OpeFqqliL4EK_=@5$Fq7`uWn z#w=h58jXOeoyvRr7C&i^+gE=w6B1-^i@*^Qp(Ks~txJ0!{j$+)N)|{1g8(J=;-@N& z=4Kn_%D9+ZWK+~$pu@~SQ4k&VN&XwHkTzrxCU}f62XGTS-~J`eoDx(4jj8xM-{#-{ z)}OUa-B`Ej_x%Lx|C#SpCsZDkre60gz=u;ipL89GfUGnPr*UW ztNiD6tXYg0ZHp-kst*MkUPwFqF1XHO@f>jiXU4D`#co{rw=h%cVc%VK#Uu@9#i0u zm6sYrJcLu_ROi_H0YjL@lcWQxtze29vkYnqBX4)?d&F@h1x^DR8(;>^)IGyYrG+ew zN$4}B+yyfMFi1q<8ag!=a1Iz*|5$dd0?t-u9ManHW^*+*htX?|?4X^;1s?ckVI1o( zp!3a72wb#h@Zk^De+P^Vu19fX&dop`blrv>2ChS}5hn3Nmy} z@_LU#TmL4^;nvXi&Ndc$s{|_O%_g#n5$WzAC7zqW` zdBW^nAF(69@4=q9Tik2xe)T9os#YKAFR;YFgB23h^58t!o7{jecarVOfqEnN{F@Ol?P*XIf!rdVI%6d! z0RuS8bsaMcfNHAn3k6P0ajSk#SnMqVpC*fl-e6YHMcHK6q*+fmUdVQ>uynqf?OPG2 zBB^>k-V%J|dX^Y`xRC3qB%}@uxl&*)G1JbqA@zv@=@giPb`;l>VO;z&1uqO4o?JpB z;S~6Ha!g=C?<=6yr3|(sm9b|nz_uq{_Fu7&kicRAjzd>t15t`H)^Fg=RDa(V#G%=c za{F!DB-*^0Zmx$dAQ96{5!uYOv5;fN*i!MaiRa-;8XCTU(K0qGplNLEBlGZVr*AIM z!% zgdKz3)K%AT?E*@HHH}P3tiagP&T`G>7UWp$yP&q?dGV;*hI5{xkT?Zs+J}6AnvJ{f zB54q~eQS}e>k%m~FVAvh<~2()`CQ3N0lt-86K7LC#;!xcIo#wlkoWbzl>?ynUZ3p% z62iMLJ%s<_TOlWPrsBSZOW8Y|xgvm1i~zLDQ`sj zh>?j?t0r;sOfI!`V9SO9jG+R2lkFyA(dyLgCenCd!0-|UXcV9&G8@s^6p)as_Yl!m zNW}NdRW0>~-vFrVPiKq@Ge%D;zpb-n)0goEe`C)L$NYyLoaF*pH%f8eev8%*9hpTx zomxYTn=$eM?jxQ#GkE6%wrJTln?z>x*aIA(3+>5ZwKHQ@ox@!-A3Haw86g=$*ILvp z)j741F=EW+Yvo?hvUJW-(6%nLu+xOmw0iXz(IDu6h3PCW0zQckNkEBX2e`I(wSgn`3y9?>NsFMdtlJSJ z6LC1*_bZXTN5=pf{9}nwDzQEG^=1L#qGZEr8lz8H(uGzF8bZ~le1ymKUH35IB$Q#- zTCS-|n;wV~Z%<@1SozTf+y#pe&~P2XFO9ZMmA7y;y$XKAchkem#G+@wH0t*eG|^~z zhQh8pNuwcL)9`C>=7xfL_sdt!pCOA9%jli)RQ01XX1O0nXRjsQ-J?kIQ>&SU>(4Ec z)WlpxlIWQEr`2IPmYhr)BU3uI+jq9=*7ULJ% z%rzmgfLBgR63>;B`Y&=)k>|_fz6NJ*Uz&bU@@D3Bp|YJhwWF@lJ16~MJ94^lOLZZ= z641OA*vib|>4!hqJ|9nmY*I6Ed~jljy~id#7vQXG9SdEQADswo#NBknz*h{;C#mutP4sX!TvH;Ym@bNpClzGP&k@@(})GxLmGESSU7nF&-Y zQ9yOJc_v=n-9s(mU)e<3hM?p(}p)UfFj8;FhlUiT1kq0OmeEgxNj5ZK%97 z6}mDsHqM2*QnWua{s(5)O0z!Q6!yKG-(0CE?|+mY3}Wkg9Bu40VMJ64%jS_@fDpGe zwgGA@l?13Vv$iYOEVc8idrC#TvKjv2ptklwHltqv7ihfW#$4e!TGrSe(6IQy%%tc* z0KDw_`|Y}=7+*_q2{K&&^<|`Z$Iv;K%3qZ;sa=&b5zySO%9(=k#L5}<^vilRWM}4D zeP|e)BPn4S&V*e10KRiL6M4~`1>!miGn8>)#^nB=hBMd3UH?l5Oq=yYz-!|!i}7o0 z#wJB>2~bJ#r)ddr-o38omI9f0B-Gze*oUWs3^)IV1MDqB_#UyDe4ccNB>)b@zJCco z#SPaIKwfVC)e@jy1K63lmH-;YrX@hNK?IKKdL?#FrYd%WlBv?F0s?Z+U?>3965HAg zWwrzIxY2M-%6lzSr1tj9ly9u2)VZ@eHnz(F$bepfE(h=x4&X};OIi{Y@f4eS2r``p z#ODcXYKdcm@$}qJsLc3BD<@_&O^bl~IZf2H^(I8>d?rpmG~9%!k(kdHY9|p1IfCVV zdi}&S8oIXLt$2O}U`P z>m#}V*4&P`pvF4Kl^4{hj~p{J|14hv9{%#TK9iIIjhDZfE_K`a>A+olXj`8|&f+fx z{yHf)qrAUW;JuRQI!42Xosb2LT_1;0#JGN@jv)Zl$8hc1G&a_o6&+bR*yF288pU{g zOQk}2{^afDN=5u&c0UbyTr1DxSoFlC7SO}Sq~Zs7*qA#CJaKp<;QH0%W!{;J}pzIQo5=o9V;DKkvC2^xeEb^lc3qWb7OQHY#lb}nR;R_Hv z%JOk~Wt#yw0JI9+{ES6OV-TRZg+oBm?e1`(lZpJR){vcPx(fBFS5yzV3N_Lg=3PF^ zMabf>XbelNC(~!9(b~0tw5*%y2B#inb0&A2x!D;eyF8g;AR>R(MRHfv>Ky1MPhDy$9l&^d*$>>&rdB?dowqEG~wYU zZCNi-Qc8> zEUEU=ML#pG;Z}@Ay=U;ZWDrcb`I@v7pq%$OUyGZ76OHJ*73vK5%GDT8crEYKA9{tW zk9z42igah*p%F$#hZ~W+l8#le6?ws-a?79}M(#4dG8jgH3`UYTs1&xA?sTayabd&* zxh63dfZS&8;>sJA=NRKAOW>r~ywASDxRh{x7vqYhm#2T{(`+V;Bjwq!DTKFZ7ue>* zc`XEh<{asvbK##HImt@B@rpXZBw9AbhM`H8noV{yM&OTU_(#6ZV{W=pGDn(sTWRPn z$AtlK2Gr2sLS4`IxLS34fWrN~Q!)jm(~Qy7y8W|&fSF*@qIj}wIJRi<%vkRyFCJ@& zZTCH2pRlyQ!SW1hAXW{ zPAc98kZ<&$+8k^~PCZ5%@AqtjQhL@p7mkY0fn86!6miNfX7@F~;27rh8xx*+=9K!> z=dR9l4?)qjsubp~^?t{uqY`&h*z+zWjh%KYvi`Q%2;H(DH*QJQ_v1cuZ}I5U61TIa zXF+sk2T*U)=DM~{NalIwM~^W*;Fhu8Z7YW%PPj|XzWBbKnfif0wA(oOYBe-ZC>zu6 zbsd!jOp_58#N}^`8)MYxoNpbBtaQ4`zv zj(tp~JR!Y>vsVtDnjHXJl?u+au_EPAmKgbBlTkp&olXsM(g9;eoFzF6Zjiu2a^Jm@ tH{{zJ95o5<#L|#CTWD@80;FjS+&^=IGt2GT%S!+N002ovPDHLkV1f&l$`k+q 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..c60920fb47 --- /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-01-07 21:47:14Functions: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..22b39a21ea --- /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-01-07 21:47:14Functions: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..2e045c255a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html @@ -0,0 +1,240 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-01-07 21:47:14Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..184fcd89b0f359d12d16f08ceb9b076b8700fff8 GIT binary patch literal 655 zcmV;A0&x9_P)0{{R38Wk~70000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpSTPn!rauAj>G z&`{Epo6QEOh>?TlaS6n$y7Q}{^By{A + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-01-07 21:47:14Functions: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)60
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()60
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().260
+
+
+ + + +
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..926d97d949 --- /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-01-07 21:47:14Functions: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)60
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()60
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().260
+
+
+ + + +
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..8b800bb6e9 --- /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-01-07 21:47:14Functions: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          60 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70          60 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71          60 :   }
+      72             : 
+      73         120 :   ~HdgPassthrough(void) {
+      74         120 :   }
+      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..24e4903973 --- /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-01-07 21:47:14Functions: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&)60
+
+
+ + + +
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..f8ab0ca67e --- /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-01-07 21:47:14Functions: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&)60
+
+
+ + + +
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..2498050385 --- /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-01-07 21:47:14Functions: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          60 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23          60 :   }
+      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..8548a97ede --- /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-01-07 21:47:14Functions: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..2ae3a8f29d --- /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-01-07 21:47:14Functions: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..cd78163385 --- /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-01-07 21:47:14Functions: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..2e6e7a4e3a --- /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-01-07 21:47:14Functions: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..62ffbca106 --- /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-01-07 21:47:14Functions: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..e01c957c1b --- /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-01-07 21:47:14Functions: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..b20978a8c5 --- /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:41269159.6 %
Date:2024-01-07 21:47:14Functions:669172.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 +
57.7%57.7%
+ 		57.7 %378 / 65570.1 %47 / 67
<unnamed>57.7 %378 / 65570.1 %47 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html new file mode 100644 index 0000000000..ee820d5df7 --- /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:41269159.6 %
Date:2024-01-07 21:47:14Functions:669172.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 +
57.7%57.7%
+ 		57.7 %378 / 65570.1 %47 / 67
<unnamed>57.7 %378 / 65570.1 %47 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html new file mode 100644 index 0000000000..93b4a47f59 --- /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:41269159.6 %
Date:2024-01-07 21:47:14Functions:669172.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 +
57.7%57.7%
+ 		57.7 %378 / 65570.1 %47 / 67
<unnamed>57.7 %378 / 65570.1 %47 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html new file mode 100644 index 0000000000..29ba9b3136 --- /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:41269159.6 %
Date:2024-01-07 21:47:14Functions:669172.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 +
57.7%57.7%
+ 		57.7 %378 / 65570.1 %47 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html new file mode 100644 index 0000000000..ac0d1d282b --- /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:41269159.6 %
Date:2024-01-07 21:47:14Functions:669172.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 +
57.7%57.7%
+ 		57.7 %378 / 65570.1 %47 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html new file mode 100644 index 0000000000..6d74bb4c43 --- /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:41269159.6 %
Date:2024-01-07 21:47:14Functions:669172.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 +
57.7%57.7%
+ 		57.7 %378 / 65570.1 %47 / 67
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html new file mode 100644 index 0000000000..d035738848 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-01-07 21:47:14Functions:5683.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.h +
100.0%
+ 		100.0 %5 / 5100.0 %3 / 3
lateral_estimator.h +
100.0%
+ 		100.0 %4 / 466.7 %2 / 3
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html new file mode 100644 index 0000000000..b402f3bf08 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-01-07 21:47:14Functions: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> ()>)60
mrs_uav_state_estimators::LatGeneric::~LatGeneric()60
mrs_uav_state_estimators::LatGeneric::~LatGeneric().260
+
+
+ + + +
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..31bda8a7fd --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-01-07 21:47:14Functions: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> ()>)60
mrs_uav_state_estimators::LatGeneric::~LatGeneric()60
mrs_uav_state_estimators::LatGeneric::~LatGeneric().260
+
+
+ + + +
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..d3a0261868 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..f922dc5e2df8de7ab7fe1ec72ccf6d7aab4af7d1 GIT binary patch literal 682 zcmV;b0#*HqP)#j#a4B1xM%6g0$cLW?W z{sYedbNSFLJhG;GaN>f2(D*bhtX%rik4O#+x40pXlovd;wa0_Wz8Os!R2G+BXPeS35%Z|eDxN8qorRp*|ALE; zf=R!COm2Vbs@H8l(IBJjM_ual^DbWv270Ul$W?%ycZ*XHjeCz1{SNtgQ{3z$)T)|Q z!1&z|@Q}{&&~0!N>0{qYqS=q7S+hT)Y>nc2C;Q3ASNwbDW4d-%w4Zll33eq5z*mcs zgu&ePx=iM5umB{bG4&Uuo40GcZuSgNfG}H3=YeJ`eS&?FYXS-oX-sQzO_~VP5GyZe zqdEf8cr9w)XZz_4w?#uHKX!YE=d}VXvYJ!}0*zt0hp@aBUM=|hx(4RJcZyF9Mic)= z)TB0kO-N;`!^KE7#%ZaTfeX^Ugy2`#$CMHJzM6H!zwDkT`*zVaomlbn7roo7qWsP3 Qf&c&j07*qoM6N<$f<09`?EnA( literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html new file mode 100644 index 0000000000..e79bb9a14e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().260
+
+
+ + + +
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..cd175cbbf1 --- /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-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().260
+
+
+ + + +
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..508db79db6 --- /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-01-07 21:47:14Functions: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          60 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24          60 :   }
+      25             : 
+      26          60 :   ~LateralEstimator(void) {
+      27          60 :   }
+      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..6ef91043e9 --- /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-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().260
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&)60
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().260
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&)107
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2107
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const97525
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&) const102164
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const102175
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const102181
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const102182
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const117759
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const117771
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const117772
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&) const168149
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const168476
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const168508
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const168512
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const1563660
+
+
+ + + +
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..dc43d25d04 --- /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-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().260
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&)107
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2107
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&)60
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().260
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() const117771
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const117772
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const117759
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&) const168149
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const168508
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const97525
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const168512
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const168476
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&) const102164
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const102181
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const1563660
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const102182
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const102175
+
+
+ + + +
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..5346e5c780 --- /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-01-07 21:47:14Functions: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         227 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62         227 :   }
+      63             : 
+      64         227 :   ~PartialEstimator(void) {
+      65         227 :   }
+      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      388466 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103      388466 :   const states_t      states = getStates();
+     104      388400 :   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     1742369 :   for (int i = 0; i < states.size(); i++) {
+     109     1353811 :     states_vec.push_back(states(i));
+     110             :   }
+     111      776686 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117      388410 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118      388410 :   const covariance_t  covariance = getCovarianceMatrix();
+     119      388331 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123     1741347 :   for (int i = 0; i < covariance.rows(); i++) {
+     124     7017274 :     for (int j = 0; j < covariance.cols(); j++) {
+     125     5663352 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128      776548 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134     1661185 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135     1661185 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141      388460 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143      388460 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147      776874 :   mrs_msgs::EstimatorOutput msg;
+     148      388355 :   msg.header.stamp    = ros::Time::now();
+     149      388472 :   msg.header.frame_id = getFrameId();
+     150      388454 :   msg.state           = getStatesAsVector();
+     151      388227 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153      388265 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160      270313 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162      270313 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166      539613 :   mrs_msgs::Float64ArrayStamped msg;
+     167      268457 :   msg.header.stamp = stamp;
+     168      640550 :   for (int i = 0; i < u.rows(); i++) {
+     169      370307 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172      269573 :   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-01-07 21:47:14Functions: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..46bcc09d67 --- /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-01-07 21:47:14Functions: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..286b5449dd --- /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-01-07 21:47:14Functions: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..fcb1828697 --- /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-01-07 21:47:14Functions: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..99116fee5a --- /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-01-07 21:47:14Functions: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..fe242d1b11 --- /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-01-07 21:47:14Functions: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..64258cb92b --- /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-01-07 21:47:14Functions: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..90cbccbf70 --- /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-01-07 21:47:14Functions: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..d93463ee28 --- /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-01-07 21:47:14Functions: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-01-07 21:47:14Functions: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)60
mrs_uav_state_estimators::StateGeneric::~StateGeneric().260
+
+
+ + + +
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..aa4736bda8 --- /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-01-07 21:47:14Functions: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)60
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::~StateGeneric().260
+
+
+ + + +
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..7f272f6ac8 --- /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-01-07 21:47:14Functions: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          60 :   StateGeneric(const std::string &name, const bool is_core_plugin)
+      79          60 :       : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) {
+      80          60 :   }
+      81             : 
+      82          60 :   ~StateGeneric(void) {
+      83          60 :   }
+      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..03695e4942 --- /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:13117475.3 %
Date:2024-01-07 21:47:14Functions: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 +
80.6%80.6%
+ 		80.6 %29 / 3633.3 %2 / 6
<unnamed>80.6 %29 / 3633.3 %2 / 6
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 4133.3 %2 / 6
proc_tf_to_world.h +
78.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
<unnamed>78.7 %37 / 4737.5 %3 / 8
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
<unnamed>72.5 %29 / 4066.7 %4 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html new file mode 100644 index 0000000000..d99d286781 --- /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:13117475.3 %
Date:2024-01-07 21:47:14Functions: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
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 4133.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
<unnamed>72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
<unnamed>78.7 %37 / 4737.5 %3 / 8
proc_median_filter.h +
80.6%80.6%
+ 		80.6 %29 / 3633.3 %2 / 6
<unnamed>80.6 %29 / 3633.3 %2 / 6
+
+
+ + + + +
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..8608046a4c --- /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:13117475.3 %
Date:2024-01-07 21:47:14Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 4133.3 %2 / 6
proc_median_filter.h +
80.6%80.6%
+ 		80.6 %29 / 3633.3 %2 / 6
<unnamed>80.6 %29 / 3633.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
<unnamed>72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
<unnamed>78.7 %37 / 4737.5 %3 / 8
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
<unnamed>70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html new file mode 100644 index 0000000000..01d5ee6535 --- /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:13117475.3 %
Date:2024-01-07 21:47:14Functions: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 +
80.6%80.6%
+ 		80.6 %29 / 3633.3 %2 / 6
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
proc_tf_to_world.h +
78.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html new file mode 100644 index 0000000000..435e9f87f3 --- /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:13117475.3 %
Date:2024-01-07 21:47:14Functions: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
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.7%78.7%
+ 		78.7 %37 / 4737.5 %3 / 8
proc_median_filter.h +
80.6%80.6%
+ 		80.6 %29 / 3633.3 %2 / 6
+
+
+ + + + +
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..31ffd5c812 --- /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:13117475.3 %
Date:2024-01-07 21:47:14Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a9577410cc42d06a4fd9ffb2a9d3eb6305b60062 GIT binary patch literal 661 zcmV;G0&4wJ+@1l& z<{5NJ`G~q?Gz+f9z%J=>jL(QF#)p9wj9IaJ!1r9V1bVs~xz?hrMMzc0dY2BAh%5QIH zV6A^Kle_LP@{ECi;o2{ZQK1sJ6r)QJ0!=tW8@R-HFwx*Sqb-vT9xC8@e$g)fy1pNG zr1yCC7Z~synmRvY009@qF!dwrM19YGzaMS8K_ZYO?x?-((YCAY;dO z?Rp9XX)=MdP6h}lG-8CdGGwY3Q^nYjtPPYmPx>S^2QKdLd=WRYUeK{Y1ho)x3a>bl z|2&0djMXkFV+?=)9V3psU!`NgF`Fq+T0(WNp-GH1G~?M>=$<{2+0h(YSZAbyrzaE8 zf2;EBynTADgN!_ClnFutK10m(aBb%M%(c*D77*y5bD*pt<=^+tFsYv@Eql7+^A=~^ zpVBCO=`lP{niywEFH9kQz literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html new file mode 100644 index 0000000000..3291b8575c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:293680.6 %
Date:2024-01-07 21:47:14Functions: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&)54
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)94545
+
+
+ + + +
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..145c991396 --- /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:293680.6 %
Date:2024-01-07 21:47:14Functions: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>&)94545
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&)54
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..d3622bb306 --- /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:293680.6 %
Date:2024-01-07 21:47:14Functions: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          54 : 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          54 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43          54 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45          54 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46          54 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48          54 :   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          54 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55          54 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58         108 :   for (int i = 0; i < n_measurements; i++) {
+      59          54 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61          54 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66       94545 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68       94545 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72       94545 :   bool ok_flag     = true;
+      73       94545 :   bool should_fuse = true;
+      74      189100 :   for (int i = 0; i < measurement.rows(); i++) {
+      75       94551 :     vec_mf_[i].add(measurement(i));
+      76       94558 :     if (vec_mf_[i].full()) {
+      77       89207 :       if (!vec_mf_[i].check(measurement(i))) {
+      78         133 :         std::stringstream ss_measurement_string;
+      79         133 :         ss_measurement_string << measurement(i);
+      80         133 :         ss_measurement_string << " ";
+      81         133 :         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         133 :         ok_flag     = false;
+      84         133 :         should_fuse = false;
+      85             :       }
+      86             :     } else {
+      87        5346 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88        5346 :       ok_flag     = false;
+      89        5346 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93       94551 :   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..9edca46fafca127b29343cb87ae828ee425aa64c GIT binary patch literal 627 zcmV-(0*w8MP)oY_Fⅅ+b3VsrtP$I;Ky3Y`nz`m9ij;SNJS-t`8W|u}d9M+T1afO; zs>_MOft&-^nfS26rRAD?NcjcVKSAvhPW)<>af zUSm<_CqUn$E=6sdv>vtzaNE8`_IT~zPgwB3+~WfT+=imI&lpgF7nrNdO6|!&Z-gJZ z{Ed0^kYUFpz#r)u0&gT-WlEXMtwGJ$XxTaEJy6>-=aNjb^g-lEj(cT2-Oc(a#92j% zcN(?$1YxkxffsvE*Fi|89@dPfiO8j}=eqdkQm{GN&^R5UDH}|QW)AH6$TO1i@q{7~ zcqEcxHUe@fr#sRSw$#ru1V*S6105et2aF1jiRA0ubyI=zsO|xcK99O_dTim~6eum% zP*_?*GcIUX<~gc3C5**Zjgwd+P}Zh?@cCg^7zsRUs%dc}edTSn5tw=cps z-7Ge|&v#F+1cS~7uL{%VC-09!+@>UE6HqKh(@#?Q?vs;I + + + + + + 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-01-07 21:47:14Functions: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)>)56
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)5547
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)97530
+
+
+ + + +
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..93ebba22f3 --- /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-01-07 21:47:14Functions: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>&)97530
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)>)56
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)5547
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..2ad1247977 --- /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-01-07 21:47:14Functions: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          60 : 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          60 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48          60 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50          60 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51          60 :   this->enabled_ = this->start_enabled_;
+      52          60 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53          60 :   ph->param_loader->loadParam("min", saturate_min_);
+      54          60 :   ph->param_loader->loadParam("max", saturate_max_);
+      55          60 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57          60 :   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          60 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      103077 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69      103077 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73      103077 :   bool ok_flag     = true;
+      74      103077 :   bool should_fuse = true;
+      75      207337 :   for (int i = 0; i < measurement.rows(); i++) {
+      76      108615 :     const double state = fun_get_state_(state_id_, i);
+      77      108614 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79      108614 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80        4350 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83      104254 :     if (measurement(i) > state + saturate_max_) {
+      84         202 :       const double saturated = state + saturate_max_;
+      85         202 :       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         202 :       measurement(i) = saturated;
+      88         202 :       ok_flag        = false;
+      89         202 :       should_fuse    = true;
+      90      104056 :     } 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       98724 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105       98724 :   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..d01c20984a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:374778.7 %
Date:2024-01-07 21:47:14Functions: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&)56
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)50376
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)98867
+
+
+ + + +
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..25f3989581 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:374778.7 %
Date:2024-01-07 21:47:14Functions: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>)50376
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)98867
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&)56
+
+
+ + + +
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..a922161f64 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html @@ -0,0 +1,222 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:374778.7 %
Date:2024-01-07 21:47:14Functions:3837.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_PROC_TF_TO_WORLD_H
+       3             : #define PROCESSORS_PROC_TF_TO_WORLD_H
+       4             : 
+       5             : #include <mrs_uav_state_estimators/processors/processor.h>
+       6             : 
+       7             : #include <sensor_msgs/NavSatFix.h>
+       8             : 
+       9             : #include <mrs_lib/gps_conversions.h>
+      10             : #include <mrs_lib/subscribe_handler.h>
+      11             : #include <mrs_lib/param_loader.h>
+      12             : 
+      13             : namespace mrs_uav_state_estimators
+      14             : {
+      15             : 
+      16             : using namespace mrs_uav_managers::estimation_manager;
+      17             : 
+      18             : template <int n_measurements>
+      19             : class ProcTfToWorld : public Processor<n_measurements> {
+      20             : 
+      21             : public:
+      22             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      23             : 
+      24             : public:
+      25             :   ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      26             :                 const std::shared_ptr<PrivateHandlers_t>& ph);
+      27             : 
+      28             :   std::tuple<bool, bool> process(measurement_t& measurement) override;
+      29             :   void                   reset();
+      30             : 
+      31             : private:
+      32             :   bool is_initialized_ = false;
+      33             : 
+      34             :   std::string gnss_topic_;
+      35             : 
+      36             :   bool   got_gnss_                  = false;
+      37             :   bool   is_gnss_offset_calculated_ = false;
+      38             :   double gnss_x_;
+      39             :   double gnss_y_;
+      40             : 
+      41             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+      42             :   void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+      43             : };
+      44             : 
+      45             : /*//{ constructor */
+      46             : template <int n_measurements>
+      47          56 : ProcTfToWorld<n_measurements>::ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      48             :                                              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+      49          56 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      50             : 
+      51          56 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      52             : 
+      53          56 :   ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      54             : 
+      55          56 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      56             : 
+      57          56 :   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          56 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          56 :   shopts.nh                 = nh;
+      65          56 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          56 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      67          56 :   shopts.threadsafe         = true;
+      68          56 :   shopts.autostart          = true;
+      69          56 :   shopts.queue_size         = 10;
+      70          56 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          56 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcTfToWorld::callbackGnss, this);
+      73             : 
+      74          56 :   is_initialized_ = true;
+      75          56 : }
+      76             : /*//}*/
+      77             : 
+      78             : /*//{ callbackGnss() */
+      79             : template <int n_measurements>
+      80       50376 : void ProcTfToWorld<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      81             : 
+      82       50376 :   if (!is_initialized_) {
+      83           0 :     return;
+      84             :   }
+      85             : 
+      86       50376 :   if (got_gnss_) {
+      87       50320 :     return;
+      88             :   }
+      89             : 
+      90          56 :   if (!std::isfinite(msg->latitude)) {
+      91           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->latitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+      92             :   }
+      93             : 
+      94          56 :   if (!std::isfinite(msg->longitude)) {
+      95           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->longitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+      96             :   }
+      97             : 
+      98          56 :   mrs_lib::UTM(msg->latitude, msg->longitude, &gnss_x_, &gnss_y_);
+      99          56 :   got_gnss_ = true;
+     100             : }
+     101             : /*//}*/
+     102             : 
+     103             : /*//{ process() */
+     104             : template <int n_measurements>
+     105       98867 : std::tuple<bool, bool> ProcTfToWorld<n_measurements>::process(measurement_t& measurement) {
+     106             : 
+     107       98867 :   if (!Processor<n_measurements>::enabled_) {
+     108           0 :     return {true, true};
+     109             :   }
+     110             : 
+     111       98867 :   if (!got_gnss_) {
+     112        1675 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     113        1675 :     return {false, false};
+     114             :   }
+     115             : 
+     116       97192 :   if (!is_gnss_offset_calculated_) {
+     117             : 
+     118          56 :     gnss_x_                    = (gnss_x_ - measurement(0)) - Processor<n_measurements>::ch_->world_origin.x;
+     119          56 :     gnss_y_                    = (gnss_y_ - measurement(1)) - Processor<n_measurements>::ch_->world_origin.y;
+     120          56 :     is_gnss_offset_calculated_ = true;
+     121             :   }
+     122             : 
+     123       97192 :   measurement(0) += gnss_x_;
+     124       97125 :   measurement(1) += gnss_y_;
+     125       97166 :   return {true, true};
+     126             : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ reset() */
+     130             : template <int n_measurements>
+     131           0 : void ProcTfToWorld<n_measurements>::reset() {
+     132           0 :   got_gnss_                  = false;
+     133           0 :   is_gnss_offset_calculated_ = false;
+     134           0 : }
+     135             : /*//}*/
+     136             : }  // namespace mrs_uav_state_estimators
+     137             : 
+     138             : #endif  // PROCESSORS_PROC_TF_TO_WORLD_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html new file mode 100644 index 0000000000..547cc3c590 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html @@ -0,0 +1,55 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c36e2d48ddbe16e7f9782c2a3dd82a0024d14e87 GIT binary patch literal 689 zcmV;i0#5yjP)v0{{R3&zjKJ0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp!uofd5*g)Qat11RYHFhk-x~tN)D!i`^E6Ah9Ea+9KyY%vB*UYHtkTKX4 z#r)W(T$7(uVnaga(JDg=~@!G6<$Qvj9xCJ0`<&ArqHB(!gCyT9OMIZo>54` zrW(`Q6{CJDH2B3GVj1UDfpqdpgrIK?A;{5y-)N7^3xG}qwKFB6OFW~;m=J9 zAB3)szOET1`$Ao(7>lk{3})j^qS%;OF&|?#Asro)U7yrP&+FSmy}hVoR>@Tz{x?Pv zU;ZAx;1qJIf17Cl&u;Xb;m^G>aB?SAZi#3wSlj_r@**U~8D_sb*Y726w?l2I0c09M zNE>5cO#GDkV*s>P{x9SEoubj!CZ>?KJ-^Q4)*{HwMbe+b@=!UB~n|2JC|-2QZC`~mp~ X<@_vk2t1d{00000NkvXXu0mjf+SEE; literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html new file mode 100644 index 0000000000..0fe022802a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const63
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const70
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const112
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&)164
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const218
+
+
+ + + +
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..eb8ba6a3bf --- /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-01-07 21:47:14Functions: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&)164
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&)60
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const112
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const218
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const70
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const63
+
+
+ + + +
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..7e6e761670 --- /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-01-07 21:47:14Functions: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         224 :   Processor(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      28             :             const std::shared_ptr<PrivateHandlers_t>& ph)
+      29         224 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30         224 :   }  // 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         281 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52         281 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58         182 : std::string Processor<n_measurements>::getPrintName() const {
+      59         182 :   return ch_->nodelet_name + "/" + correction_name_ + "/" + name_;
+      60             : }
+      61             : /*//}*/
+      62             : 
+      63             : /*//{ toggle() */
+      64             : template <int n_measurements>
+      65           0 : void Processor<n_measurements>::toggle(bool enable) {
+      66           0 :   enabled_ = enable;
+      67           0 : }
+      68             : /*//}*/
+      69             : 
+      70             : }  // namespace mrs_uav_state_estimators
+      71             : 
+      72             : #endif  // PROCESSORS_PROCESSOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html new file mode 100644 index 0000000000..804fb09cf2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html @@ -0,0 +1,38 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5bde3e19271072329a6f5996af74c7f1cd4876bb GIT binary patch literal 436 zcmV;l0ZaagP)Ir7GwQk%4-g?oskOT)R(R$vYpnS+meZL^Ea=M0kJaxFSTc z32H$x9|fX3-#CC~#B?1rr;fx2&axu4p2Hm<8V4}X(NOjTn@>QGHiL*QhiTGJcM{QI zcEYd_=+;(>BBbidsb8hWiI0LEB;K?W4L$!}-{}$`SjR_JkDuK120ePg%olHM&$>x#~P3jjH@25qq;e;xFU5X@e8u zL5hL(9u|*xjz%}DKZxi46(Npi-`BmtM&E7WPzx=rL|%qa+ju<_$wWjHLqq#-Pk!}9 ej;eT@U+_NbxtO|J!@K?f0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7310470.2 %
Date:2024-01-07 21:47:14Functions: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()47
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&)47
mrs_uav_state_estimators::garmin_agl::GarminAgl::start()47
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)826
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)79755
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const84264
+
+
+ + + +
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..c6b38e7482 --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()47
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&)47
mrs_uav_state_estimators::garmin_agl::GarminAgl::isConverged()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)79755
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)826
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()47
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const84264
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..a6f3cdf6ee --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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          47 : void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          47 :   ch_ = ch;
+      17          47 :   ph_ = ph;
+      18          47 :   nh_ = nh;
+      19             : 
+      20          47 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      21             : 
+      22             :   // | --------------------- load parameters -------------------- |
+      23             : 
+      24          47 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          47 :   if (is_core_plugin_) {
+      27             : 
+      28          47 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      29          47 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          47 :   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          47 :   _update_timer_rate_       = 100;                                                                                          // TODO: parametrize
+      39          47 :   timer_update_             = nh.createTimer(ros::Rate(_update_timer_rate_), &GarminAgl::timerUpdate, this, false, false);  // not running after init
+      40          47 :   _check_health_timer_rate_ = 1;                                                                                            // TODO: parametrize
+      41          47 :   timer_check_health_       = nh.createTimer(ros::Rate(_check_health_timer_rate_), &GarminAgl::timerCheckHealth, this);
+      42             : 
+      43             :   // | --------------- subscribers initialization --------------- |
+      44          94 :   mrs_lib::SubscribeHandlerOptions shopts;
+      45          47 :   shopts.nh                 = nh;
+      46          47 :   shopts.node_name          = getPrintName();
+      47          47 :   shopts.no_message_timeout = ros::Duration(0.5);
+      48          47 :   shopts.threadsafe         = true;
+      49          47 :   shopts.autostart          = true;
+      50          47 :   shopts.queue_size         = 10;
+      51          47 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      52             : 
+      53             :   // | ---------------- publishers initialization --------------- |
+      54          47 :   ph_agl_height_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, Support::toSnakeCase(getName()) + "/agl_height", 10);
+      55          47 :   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          47 :   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          47 :   est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName(), is_core_plugin_);
+      65          47 :   est_agl_garmin_->initialize(nh, ch_, ph_);
+      66             : 
+      67          47 :   max_flight_z_ = est_agl_garmin_->getMaxFlightZ();
+      68             : 
+      69             :   // | ------------------ initialize published messages ------------------ |
+      70          47 :   agl_height_init_.header.frame_id     = ns_frame_id_;
+      71          47 :   agl_height_cov_init_.header.frame_id = ns_frame_id_;
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          47 :   if (changeState(INITIALIZED_STATE)) {
+      76          47 :     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          47 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84          47 : bool GarminAgl::start(void) {
+      85             : 
+      86             : 
+      87          47 :   if (isInState(READY_STATE)) {
+      88             : 
+      89             :     bool est_agl_garmin_start_successful;
+      90             : 
+      91          47 :     if (est_agl_garmin_->isStarted() || est_agl_garmin_->isRunning()) {
+      92           0 :       est_agl_garmin_start_successful = true;
+      93             :     } else {
+      94          47 :       est_agl_garmin_start_successful = est_agl_garmin_->start();
+      95             :     }
+      96             : 
+      97          47 :     if (est_agl_garmin_start_successful) {
+      98          47 :       timer_update_.start();
+      99          47 :       changeState(STARTED_STATE);
+     100          47 :       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       79755 : void GarminAgl::timerUpdate(const ros::TimerEvent &event) {
+     147             : 
+     148       79755 :   if (!isInitialized()) {
+     149           0 :     return;
+     150             :   }
+     151             : 
+     152       79755 :   const ros::Time time_now = ros::Time::now();
+     153             : 
+     154      159510 :   mrs_msgs::Float64Stamped agl_height = agl_height_init_;
+     155       79755 :   agl_height.header.stamp             = time_now;
+     156       79755 :   agl_height.value                    = est_agl_garmin_->getState(POSITION);
+     157             : 
+     158      159510 :   mrs_msgs::Float64ArrayStamped agl_height_cov;
+     159       79755 :   agl_height_cov.header.stamp = time_now;
+     160             : 
+     161       79755 :   const int n_states = 2;  // TODO this should be defined somewhere else
+     162       79755 :   agl_height_cov.values.resize(n_states * n_states);
+     163       79755 :   agl_height_cov.values.at(n_states * POSITION + POSITION) = est_agl_garmin_->getCovariance(POSITION);
+     164       79755 :   agl_height_cov.values.at(n_states * VELOCITY + VELOCITY) = est_agl_garmin_->getCovariance(VELOCITY);
+     165             : 
+     166       79755 :   mrs_lib::set_mutexed(mtx_agl_height_, agl_height, agl_height_);
+     167       79755 :   mrs_lib::set_mutexed(mtx_agl_height_cov_, agl_height_cov, agl_height_cov_);
+     168             : 
+     169       79755 :   publishAglHeight();
+     170       79755 :   publishCovariance();
+     171       79755 :   publishDiagnostics();
+     172             : }
+     173             : /*//}*/
+     174             : 
+     175             : /*//{ timerCheckHealth() */
+     176         826 : void GarminAgl::timerCheckHealth(const ros::TimerEvent &event) {
+     177             : 
+     178         826 :   if (!isInitialized()) {
+     179           0 :     return;
+     180             :   }
+     181             : 
+     182         826 :   if (isInState(INITIALIZED_STATE)) {
+     183             : 
+     184          47 :     if (est_agl_garmin_->isReady()) {
+     185          47 :       changeState(READY_STATE);
+     186          47 :       ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
+     187             :     } else {
+     188           0 :       ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     189           0 :       return;
+     190             :     }
+     191             :   }
+     192             : 
+     193             : 
+     194         826 :   if (isInState(STARTED_STATE)) {
+     195          48 :     ROS_INFO("[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     196             : 
+     197          48 :     if (est_agl_garmin_->isRunning()) {
+     198          47 :       ROS_INFO("[%s]: Subestimators converged", getPrintName().c_str());
+     199          47 :       changeState(RUNNING_STATE);
+     200             :     } else {
+     201           1 :       return;
+     202             :     }
+     203             :   }
+     204             : }
+     205             : /*//}*/
+     206             : 
+     207             : /*//{ isConverged() */
+     208           0 : bool GarminAgl::isConverged() {
+     209             : 
+     210             :   // TODO: check convergence by rate of change of determinant
+     211             :   // most likely not used in top-level estimator
+     212             : 
+     213           0 :   return true;
+     214             : }
+     215             : /*//}*/
+     216             : 
+     217             : /*//{ getUavAglHeight() */
+     218       84264 : mrs_msgs::Float64Stamped GarminAgl::getUavAglHeight() const {
+     219       84264 :   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          47 : 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..f80a68d47b6e7f790ea2dc61186bcd6cec72c8a6 GIT binary patch literal 1050 zcmV+#1m*jQP)@by?R{USHx} za_M`N5+WxwPQ0hTMhRC%0pD&JnC<{SVA_R6c7w2OdMiY4X1gCigkp+Nn=zaD4v;CA zMz>ELf4X0DBYcgQ$3Z46KJMuyIUHkE#^qU}xgV2}iLusgAX_7`NXRAa*2o1i;{2E% z@MDGH6%UAgKfT!dVbA0G3V-|O`vm~pn$n`TKV0MHn)K3FUzG z;B1(W7X@ijP1r@IkE3CD#Pm!7Xgi!OD(<%$ z69Xbs{EMz-*YGi1S~fY5t1+*S-aiX!0+(8)P%2Qfx<*9PI&gN4WDOq(VV+fW@}^_h zq|o9vhYi;YFv3s|z^dctm?;*YzCA*4QweZ-zeD zA7Hf_=fHRGT1Zz=yHZpN8w!Oz+~1*6U`~*AgI`j1SB$!F!P9r2*YPa5>&3F-41(F? zF?~03{rz%wRx~op9xCYv?M}WQcXUY5Ix|u^1fSE)|4Lc*;n;8lS2UGMcCB0U;#ZIpggt`MX)%7{xfN2xF zZM5-Yc-K0INYvH{9NFGXfVp6TNz4Fn(vQiP;nLXv;tccf8Pn}v7n)`Rn=1Kt*H0b8 zzM{2sS3dF4;AgzC{BPBaIacKFI#=1yh+qwk*?307>`zn`iaDME_v(>;~@D-_5PUG$=Z9kY_>_ z0Qeq2++T8DH}{>WcxGbJNHoN{6vXH6##%bkv;J4ighnr~X^d&t(^xXBkf)yi0E=}5 UYtfw{-T(jq07*qoM6N<$g0Eoi=Kufz 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..fcb2d8082c --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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 +
70.2%70.2%
+ 		70.2 %73 / 10460.0 %6 / 10
<unnamed>70.2 %73 / 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..8a03dea70e --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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 +
70.2%70.2%
+ 		70.2 %73 / 10460.0 %6 / 10
<unnamed>70.2 %73 / 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..33ec8a168f --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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 +
70.2%70.2%
+ 		70.2 %73 / 10460.0 %6 / 10
<unnamed>70.2 %73 / 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..23085f1005 --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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 +
70.2%70.2%
+ 		70.2 %73 / 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..321fa28d02 --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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 +
70.2%70.2%
+ 		70.2 %73 / 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..99ccab9cb3 --- /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:7310470.2 %
Date:2024-01-07 21:47:14Functions: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 +
70.2%70.2%
+ 		70.2 %73 / 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..c959a7adb1 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)107
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)107
mrs_uav_state_estimators::AltGeneric::isConverged()108
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)165
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const698
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const873
mrs_uav_state_estimators::AltGeneric::start()4459
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const97529
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) const97531
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const112247
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)168460
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const168482
mrs_uav_state_estimators::AltGeneric::setDt(double const&)168483
mrs_uav_state_estimators::AltGeneric::getStates() const168514
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)205747
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) const275386
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&)275395
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)275408
mrs_uav_state_estimators::AltGeneric::generateB()336994
mrs_uav_state_estimators::AltGeneric::generateA()337004
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const383942
mrs_uav_state_estimators::AltGeneric::getState(int const&) const681632
+
+
+ + + +
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..6c3431d291 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::AltGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)107
mrs_uav_state_estimators::AltGeneric::isConverged()108
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)205747
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&)275395
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)275408
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)168460
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)107
mrs_uav_state_estimators::AltGeneric::setCovarianceMatrix(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)0
mrs_uav_state_estimators::AltGeneric::pause()0
mrs_uav_state_estimators::AltGeneric::reset()0
mrs_uav_state_estimators::AltGeneric::setDt(double const&)168483
mrs_uav_state_estimators::AltGeneric::start()4459
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)165
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()337004
mrs_uav_state_estimators::AltGeneric::generateB()336994
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const873
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const383942
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const112247
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const698
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const168482
mrs_uav_state_estimators::AltGeneric::getState(int const&) const681632
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const97529
mrs_uav_state_estimators::AltGeneric::getStates() const168514
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) const275386
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) const97531
+
+
+ + + +
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..92e88fd566 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html @@ -0,0 +1,797 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..44ae394feaddfa93f5099c1ad01a1ccad5a09be2 GIT binary patch literal 2797 zcmV};WzVUg=Y+=TI_(}EAyYrJ!E%rF#E&X`L)3-Q8B92XZyuO zHtLa$!3dUX?AGTAlx;o->j{aEGJI_|;y$l+xkwnpyyj>C=qX-|BUYe(nCiWLw8y$zI3$xCa#NKF znKk21R907H9`uw2%oqV9wJLxmHB~?tXH?eaOE&?X6`3$8=eWy&#(|cu$dF?{1a~5kdfu>%*sC)J zeA;_7jy=9MhX9z2AeOU>NX|!kBJZ)qaJ!{2D_P@A{a~2IjH=M8>x@mcJRq5|bb-l? z4X2C%uC0KyXOCv-NlUjZi{1A8^+KTvM4S=){KM9R>3^eh^a9S9hf!{OeN)k>_o)%*$ZJWx2xaLH4T-$jSsW`WJa%9bSrcAdgFT%%ia`>L2xy?= zWMuXy$kg!(dyMtB>{(|#O2AYIgkBPP#;~{)vU9+>9Ym(;9?YjyvvL`*95mNZ06O?O zXn>#tpe4>_OM_TAJ|hfavk|R4V>_OiZ1Usf!e*L_jn(2p9-TR+2yVKGDzr?qz(5jX ziY#o*n9raFAY0pI>CA%xRx+AKLjD@g*lmv?v&hsn>%H^5h7uSzDHVcWqJ@ST&*uy} z*jwQqJJ{d(bVuItZ#~$PMC4N<8GtA>kQ0}tCI+A>j|6O%Gg;gK*Z>_VHG*gT*6T%c}qK?9`MGcvon3Nzx+KlI#6!R#NcFH-1$O8^!&df%&QRsMJ zUecWTW=Z$lD<8*Uh~A#2g@V%jgy56SD^%1~o6)dx*Xha&s_Gt1OHbz?NEdY3S?n#T zbJaW4F9^gBTjs47fy_-_) z0nC8rI~Ao(NY`zsy$FDGe4ACKZ)SepN}OdcDYdBmnBcJ!96NHhR**=ff5Vtcy7%*{j5O=+1bknpUE zir^X>&g7=p75d#%h9Ce+MYuAB;Ij&~LO1|U(}YaQ(PW|^bGDBg3y>-AqGn!vcE%H9 zjnHx}bt#JRE2dnJE=6P@X<_^jy;;)X0*k0FHvq$Wa9XXx6j zv2dqf38?bM1DXi=YdB-KJ%%Ph0Fp$I zp2P!?gc@?E;hh9GblPe&o-4Qm_bEoW#WzBPz>rpqBsY7C9)8vAonG-Nh*J%lfytHa zUUQ$zR`CI3AV`pW#6ff^Ht@}k!~i{piID3vYC6zOp08h(XvV@q&#-dIktQ@|d^zsZ z96&3ZkOBV}tX*n{$h~f^@A+o;Z-5SI?nih45=3v1<9x)_xAIhunM=l(k%_Um=hC>>9b-f@?2G8m95KzhCR4h-5#U+H*c@*cYMV>sz1$!LqKB*BdTQjBE3yjo74-d2z7Ol>Q zu?*Nj9$UdM0QxtrvUu*tZ9O700=mi*J`kX#R@rk`s?13xy@dyOB?&jxDq}|_u2n`N z2(@7=uTR#5zqF?5NPJL{@CyTaNWWkyk-+ zmkZRSciNTrG{J&@jJ0{Fdo1qhiF-uf|1Qpy4qWz=puiAaE@EM@y(yj(XTB|Mgj-5= zMVAXOlW4C(hQyXA-_p22o}$_GB?=dl#)>?$J>Ebbn^<%40&jsz&zlbLmka4J%j4VB zpV4z@qUVm)@n%|;Px$N+gm+~thpL8ltj$A|vefR1QVR|H2oDmygFH5wBO}3;@mFbK z6Khe(R?xHLp^I9$35xV9w6G%K#ONwsN^$SIi`VAyBTFGZcjlWunEiH;o~ zH4?4R*?_H1-~-UIOFQ5V+!=#%3D!}#}H|CBPCi`!P<(9{lj#O z9~TpoS1x_DKm*Y581D=iCAD(fCEc`dW#azv`aE8nmhJ*<@%aG!MZ7lnxPmi-&2+TW ze^y1@_g$I%o(25g;48&&^v8g?U2JOtCjU))h~f90ID-!m6^0JSC8Tt(;bR+0Uf&{= zYPOp&ti|VkQLN{=sM`YxF2MR3Z8N?)DE;Ms^*j`}LayGY00000NkvXXu0mjf8b46= literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html new file mode 100644 index 0000000000..b4e30f14c3 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
<unnamed>62.8 %233 / 37173.3 %22 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html new file mode 100644 index 0000000000..8ae56a7a2c --- /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:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
<unnamed>62.8 %233 / 37173.3 %22 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-detail.html b/mrs_uav_state_estimators/src/estimators/altitude/index-detail.html new file mode 100644 index 0000000000..ac881adcd9 --- /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:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
<unnamed>62.8 %233 / 37173.3 %22 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html new file mode 100644 index 0000000000..d52daa47be --- /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:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html new file mode 100644 index 0000000000..1e81776741 --- /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:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index.html b/mrs_uav_state_estimators/src/estimators/altitude/index.html new file mode 100644 index 0000000000..e7d9b68d97 --- /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:23337162.8 %
Date:2024-01-07 21:47:14Functions:223073.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
alt_generic.cpp +
62.8%62.8%
+ 		62.8 %233 / 37173.3 %22 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..5292b0e8e9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:03350.0 %
Date:2024-01-07 21:47:14Functions:0310.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..0b09042fa36a2f106e7a304e58266487570d373d GIT binary patch literal 2195 zcmV;E2yFL>P)2nt zU)m3A)4%)2dJ%latVl(VUKl_c!Xrkg5)Z5AOhSBV3s~CEh*2If;fdZAW7H=FgE>d8 zdDx##b~byC)P}%E7~VGq2q6P*3df~p#}*YF{lSo}$?ow9VLJaPS%zOau9_v4cnpf6 zJR($IpQ5(*zddHcG>0=6Sf)mJLN=zyEFcjEt+N; z)Z_Q-GiUwH$=#?PSq_<{3HRTm=L<$hrP;o_wFveKk5`5i3ZEQP@}{|wKnNEjfF*jf zG&DzC@+vq&m)7bW!i)uB_IZJjz(g{A$Z-K-V5v=Mt2LmP zBZN0ibZNgQSo_tEE7o_eCfwxi>N*py#;FRDf$QD2AjI4dcS4g;(T~xA(DRfnBoT(+ zSaI9@T6HvaEq5u7&ujCZBB7V^$C}AXXrC5lrO5jE5+32PpPNlF()ub*4-_2dtYr#A z$cOi-+-BOex+Sc&DH6Icqp)_nv`XkuVf>`6#a$u<2U)s@pMwXVxeJQH(}%*K-(68J ztd@4N!gu}j+$E$l$CegXCYh9Knfqc7Jlgz`mz+qQzU@K476&9ZZRqb7D8w0EI3TU% zsR`F3NN1)MDZ`l&%fxI|B`jr=6*W{hgMmvZ9&Pt}+~WFv;` zUL#06Y6iz)20NLurydxOftenfpO2mw6o0I60tz|sn7aJn*0Q!nHjoP}0}l|l<9&sB zuY&E{w6yfUb7G&;q2uEV6+MODc(gx!$dw7PL+)k3O{rOtm>yEk3Pt9Dly0ywk zbGYvzgY<^;lud%`+xx~qx#mWQUWxe)h2tRwIBYgv82xxNWmJ9gYXrKNITtYFvs_^< z_BrYbw+l1AUq?g~x$#a|9Ekah_esF@Cq8M$bAdY*r`K?iBXyJJoX^)nJ0V)OVNdYG zTf^Q3mu$++NP=(KSLm>NO<%zu*VVhgS(t2PF7@Nevo36hE+$DC;rl2+F3%|Wz;)_L zQrlRz>J$rZJMPJWspV1C;@!e*y3Q~t5m8*dwB9Jdq2hkaL|H6J3is+9;Ssu0eMJDs z&wVVUX4#Zh_L^R}%on?kTmfd8w!5`+>P_ZRk$(wiX_AU?`s7|`n)=g(EKk^uPxZ+! z-TpPN9*hnj>57{|XZ9i`0E&bu&9Cu`B6FKv!?ygD{*ir>R zix)N^Y@_fx9|)kZ^+l7WcJ7P&Adevh&`?=4ayojN>Qq=VCx6u~?zE&6v?rPjdYHlx z2A^uiy>%=Qo}E7$lJ)!Cr~_Q}e}?SU-QBb4&y?nKGyq`2y#1NCKlAoyBH`Pg zdHXXBkL&%e@b+go!nZ%uSNPkXdHXZ%j1K|sFZ5@mAo=!Zy6}DbGX==y8Bgoa+qh;=j>~X?Sc{ zWdK)?@6UYv`k#e7)ENr4pTL&zJ^0zp@5c!+y%>kOEabTKdrjzJVx~3Gtwd#Wwfz1S-_@r&Tm9EXJCZT+8%;OPQ$k) z9~1t_CfJ{WnKTT1L_KaFw60j3(P^(Y2!yWlKs6yen!1K!0d2PL=z)4e;jCcEY;nT1 zAw;)f*fIp*T^`ZU{Qqn=J)!rZ-D+4Ne2?yh>57KW3`ml4NY0;l7%0BeK$~~1G@f3A zlq${sldJnPiM)C31S~Z?L>{a9Jj)~b|F2o55<1mfN+Z!%FjbP5V&YPRbOz#avk()c z*0c%vkHb8!y<~?H4qHaOdBuXOL1PDx4UZ4NBOrAnZEP3Rw{aL-Gjcz{)~ejB1!GM$ zeGYF-V^0Z{8~%m(NO^R{g#iJN)J~g~o`6TyIJ^+_N476@Bg}6|&C43?7XuLMh5qZX z)XX3Aga@QP?uA9rmAtTQ3=c^Sj}DJ|q+lQOza5X`@6gx}Jsgh%?>P74d_v`59|#dH zal+qlE+#rMPj*!Aitm-0UK92RhfO$`o1O6{8~iQ)18<*~$|oFQ#+po+Ha=y>*M#Sp z5L(*AdYIZ9{vi{pR&4im{{EnRR;a?;{TaZ~j5H3Hh5EOepGJjj5+>;(boh8)MhP=y z+8SISdr*j literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html new file mode 100644 index 0000000000..1c4ed789d5 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8813167.2 %
Date:2024-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::HdgPassthrough::start()100
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const120
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const337
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)106222
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)108737
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const117756
mrs_uav_state_estimators::HdgPassthrough::getStates() const117770
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)117773
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)119007
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)214946
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const412385
+
+
+ + + +
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..ee5cf081bc --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8813167.2 %
Date:2024-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)117773
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)119007
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)108737
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>)106222
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()100
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)214946
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]() const337
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]() const120
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const117756
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const412385
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const117770
+
+
+ + + +
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..9c4935064d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html @@ -0,0 +1,387 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8813167.2 %
Date:2024-01-07 21:47:14Functions: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          60 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          60 :   ch_ = ch;
+      17          60 :   ph_ = ph;
+      18             : 
+      19          60 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21          60 :   hdg_state_      = states_t::Zero();
+      22          60 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24             :   // | --------------- param loader initialization --------------- |
+      25             : 
+      26          60 :   if (is_core_plugin_) {
+      27             : 
+      28          60 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      29          60 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          60 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      33             : 
+      34             :   // | --------------------- load parameters -------------------- |
+      35          60 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      36             : 
+      37          60 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      38          60 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      39             : 
+      40          60 :   if (!ph->param_loader->loadedSuccessfully()) {
+      41           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+      42           0 :     ros::shutdown();
+      43             :   }
+      44             : 
+      45             :   // | ------------------ timers initialization ----------------- |
+      46          60 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      47          60 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      48             : 
+      49             :   // | --------------- subscribers initialization --------------- |
+      50             :   // subscriber to odometry
+      51         120 :   mrs_lib::SubscribeHandlerOptions shopts;
+      52          60 :   shopts.nh                 = nh;
+      53          60 :   shopts.node_name          = getPrintName();
+      54          60 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      55          60 :   shopts.threadsafe         = true;
+      56          60 :   shopts.autostart          = true;
+      57          60 :   shopts.queue_size         = 10;
+      58          60 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      59             : 
+      60         120 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      61          60 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      62         120 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      63          60 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      64             : 
+      65             :   // | ---------------- publishers initialization --------------- |
+      66          60 :   if (ch_->debug_topics.output) {
+      67          60 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      68             :   }
+      69          60 :   if (ch_->debug_topics.diag) {
+      70           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, getNamespacedName() + "/diagnostics", 10);
+      71             :   }
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          60 :   if (changeState(INITIALIZED_STATE)) {
+      76          60 :     ROS_INFO("[%s]: Estimator initialized, version %s", getPrintName().c_str(), VERSION);
+      77             :   } else {
+      78           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      79             :   }
+      80          60 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84         100 : bool HdgPassthrough::start(void) {
+      85             : 
+      86         100 :   if (isInState(READY_STATE)) {
+      87          60 :     timer_update_.start();
+      88          60 :     changeState(STARTED_STATE);
+      89          60 :     return true;
+      90             : 
+      91             :   } else {
+      92          40 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      93          40 :     return false;
+      94             :   }
+      95             : }
+      96             : /*//}*/
+      97             : 
+      98             : /*//{ pause() */
+      99           0 : bool HdgPassthrough::pause(void) {
+     100             : 
+     101           0 :   if (isInState(RUNNING_STATE)) {
+     102           0 :     changeState(STOPPED_STATE);
+     103           0 :     return true;
+     104             : 
+     105             :   } else {
+     106           0 :     return false;
+     107             :   }
+     108             : }
+     109             : /*//}*/
+     110             : 
+     111             : /*//{ reset() */
+     112           0 : bool HdgPassthrough::reset(void) {
+     113             : 
+     114           0 :   if (!isInitialized()) {
+     115           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     116           0 :     return false;
+     117             :   }
+     118             : 
+     119           0 :   changeState(STOPPED_STATE);
+     120             : 
+     121           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     122             : 
+     123           0 :   return true;
+     124             : }
+     125             : /*//}*/
+     126             : 
+     127             : /*//{ callbackOrientation() */
+     128      108737 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     129             : 
+     130      108737 :   if (!isInitialized()) {
+     131           3 :     return;
+     132             :   }
+     133             : 
+     134             :   double hdg;
+     135             :   try {
+     136      108725 :     hdg = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     137             :   }
+     138           0 :   catch (...) {
+     139           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     140             :   }
+     141             : 
+     142      108721 :   setState(hdg, POSITION);
+     143             : 
+     144      108695 :   if (!isError()) {
+     145      108693 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     146             :   }
+     147             : }
+     148             : /*//}*/
+     149             : 
+     150             : /*//{ callbackAngularVelocity() */
+     151      106222 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     152             : 
+     153      106222 :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     154           0 :     return;
+     155             :   }
+     156             : 
+     157             :   double hdg_rate;
+     158             :   try {
+     159      106212 :     hdg_rate = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+     160             :   }
+     161           0 :   catch (...) {
+     162           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     163             :   }
+     164             : 
+     165      106058 :   setState(hdg_rate, VELOCITY);
+     166             : }
+     167             : /*//}*/
+     168             : 
+     169             : /* timerUpdate() //{*/
+     170      117773 : void HdgPassthrough::timerUpdate(const ros::TimerEvent &event) {
+     171             : 
+     172             : 
+     173      117773 :   if (!isInitialized()) {
+     174           0 :     return;
+     175             :   }
+     176             : 
+     177      117766 :   publishOutput();
+     178      117773 :   publishDiagnostics();
+     179             : }
+     180             : /*//}*/
+     181             : 
+     182             : /*//{ timerCheckHealth() */
+     183      119007 : void HdgPassthrough::timerCheckHealth(const ros::TimerEvent &event) {
+     184             : 
+     185      119007 :   if (!isInitialized()) {
+     186          16 :     return;
+     187             :   }
+     188             : 
+     189      118978 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     190             : 
+     191          60 :     changeState(READY_STATE);
+     192          60 :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     193             :   }
+     194             : 
+     195      118949 :   if (isInState(STARTED_STATE)) {
+     196             : 
+     197          60 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     198          60 :     changeState(RUNNING_STATE);
+     199             :   }
+     200             : 
+     201      118974 :   if (sh_orientation_.hasMsg()) {
+     202      118807 :     is_orient_ready_ = true;
+     203             :   } else {
+     204         181 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     205             :   }
+     206             : 
+     207      118997 :   if (sh_ang_vel_.hasMsg()) {
+     208      117851 :     is_ang_vel_ready_ = true;
+     209             :   } else {
+     210        1076 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity yet", getPrintName().c_str());
+     211             :   }
+     212             : }
+     213             : /*//}*/
+     214             : 
+     215             : /*//{ getState() */
+     216           0 : double HdgPassthrough::getState(const int &state_id_in, const int &axis_in) const {
+     217           0 :   return getState(stateIdToIndex(state_id_in, 0));
+     218             : }
+     219             : 
+     220      412385 : double HdgPassthrough::getState(const int &state_idx_in) const {
+     221      412385 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     222             : }
+     223             : /*//}*/
+     224             : 
+     225             : /*//{ setState() */
+     226           0 : void HdgPassthrough::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     227           0 :   setState(state_in, stateIdToIndex(state_id_in, 0));
+     228           0 : }
+     229             : 
+     230      214946 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     231             : 
+     232      214946 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     233      214799 :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     234      214822 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     235             : 
+     236      214782 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     237      214715 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     238      214808 : }
+     239             : /*//}*/
+     240             : 
+     241             : /*//{ getStates() */
+     242      117770 : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     243      117770 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_);
+     244             : }
+     245             : /*//}*/
+     246             : 
+     247             : /*//{ setStates() */
+     248           0 : void HdgPassthrough::setStates(const states_t &states_in) {
+     249           0 :   mrs_lib::set_mutexed(mtx_hdg_state_, states_in, hdg_state_);
+     250           0 : }
+     251             : /*//}*/
+     252             : 
+     253             : /*//{ getCovariance() */
+     254           0 : double HdgPassthrough::getCovariance(const int &state_id_in, const int &axis_in) const {
+     255           0 :   return getCovariance(stateIdToIndex(state_id_in, 0));
+     256             : }
+     257             : 
+     258           0 : double HdgPassthrough::getCovariance(const int &state_idx_in) const {
+     259           0 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_(state_idx_in, state_idx_in));
+     260             : }
+     261             : /*//}*/
+     262             : 
+     263             : /*//{ getCovarianceMatrix() */
+     264      117756 : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     265      117756 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_);
+     266             : }
+     267             : /*//}*/
+     268             : 
+     269             : /*//{ setCovarianceMatrix() */
+     270           0 : void HdgPassthrough::setCovarianceMatrix(const covariance_t &cov_in) {
+     271           0 :   mrs_lib::set_mutexed(mtx_hdg_covariance_, cov_in, hdg_covariance_);
+     272           0 : }
+     273             : /*//}*/
+     274             : 
+     275             : /*//{ getInnovation() */
+     276           0 : double HdgPassthrough::getInnovation(const int &state_idx) const {
+     277           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_(state_idx));
+     278             : }
+     279             : 
+     280           0 : double HdgPassthrough::getInnovation(const int &state_id_in, const int &axis_in) const {
+     281           0 :   return getInnovation(stateIdToIndex(state_id_in, 0));
+     282             : }
+     283             : /*//}*/
+     284             : 
+     285             : /*//{ getLastValidHdg() */
+     286           0 : double HdgPassthrough::getLastValidHdg() const {
+     287           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     288             : }
+     289             : /*//}*/
+     290             : 
+     291             : /*//{ getNamespacedName() */
+     292         120 : std::string HdgPassthrough::getNamespacedName() const {
+     293         240 :   return parent_state_est_name_ + "/" + getName();
+     294             : }
+     295             : /*//}*/
+     296             : 
+     297             : /*//{ getPrintName() */
+     298         337 : std::string HdgPassthrough::getPrintName() const {
+     299         674 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     300             : }
+     301             : /*//}*/
+     302             : 
+     303             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html new file mode 100644 index 0000000000..a0f4905b86 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html @@ -0,0 +1,96 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..d11291d3d4e80dd6164377556c01c1c9fb3b1924 GIT binary patch literal 1189 zcmV;W1X}xvP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpKAk1!HSC9?D3U&kBf0YyYg-=58>8p2_>sh5~AYYQu>HU7s{{KBsSHSyq z(3{_vP&+8@V2N@tNbwq0i#5*Y7u0~umpFML&RtDwt#kw67)KvXix8$o18BGuQ9ktn zO2v0~TTU`caleBo8Fp&RlIBq5Q|~$qp71(eb9!>`qeihLg({wF=^B?s_u$aL0HVMk zoOtA1H^JH-;DJ+`In*&{YS|Svvr~sz6Q{!#-X%&S>g3iiICV2pRF*WY>*nEOU!TqF zf8p6*?EpetS(sMCBSb_wAQjF@c{8O#D=KKQAEbZ;4jn|P!1Dl5FEXytNBh}r$0M#_ zHAw9DFnOh@lS6ZO+p+4DsTEfYR8WV6x`L z<_NVM{ly0R4-WUEaulk1nKbG01)^LrzCh|CmRU0;s|~ui^#q8`EO(*(iSiklYgxpcqM@7;w=A79A(r-1W>)8< zT_ehay@~FfEPgnN6GN!41?h~C>tH>s=`3{f3Xtu3#9xzB2ppDh*J}8byQskwZe)kl=OtL~JpUXFjzU4yyMMTNMEyTKJ+v1!I5He5lW_h`KXg^v`(Tih3Y zCU3AYYeaFBRr^#G{N4psx0lj;D{jGGS>L8^^j$*nxeKbTbxYYvy-;>I|e?Qk; zqw);b3isKvopGc)^G&WJCv1C2-Yu-f15Pk1jJTV-xn}$jtr6%$h3mLh+ME0c*AGZE z*&u3RD;pK*9MHsjW3!YR(|uCzT;gh247!T;5FJ%z89_o@OP${8{LQc#RM6!YG`g>wmsDsZ9y8^$)RHJii%;1;waXCLs&PaG69(;}$cDZI1czD<+>AHgKG{6kn zA_%EIxG8RJ?E>>BTl6t~(VubLJwNFiyBB-d&5NnG=6H&Pcrl2>YF>`6c3 zEJMm8VoxU%Z*r~bqB7??{;Iha!B@HVeOLVJ{nive>hpUUg(oE<00000NkvXXu0mjf D9-b<6 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html new file mode 100644 index 0000000000..634b1481ab --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2024-01-07 21:47:14Functions:125422.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
<unnamed>67.2 %88 / 13152.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html new file mode 100644 index 0000000000..2470f42373 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2024-01-07 21:47:14Functions:125422.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
<unnamed>67.2 %88 / 13152.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail.html new file mode 100644 index 0000000000..21681bc6db --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2024-01-07 21:47:14Functions:125422.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
<unnamed>67.2 %88 / 13152.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html new file mode 100644 index 0000000000..563fd142d0 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2024-01-07 21:47:14Functions:125422.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html new file mode 100644 index 0000000000..aaf4e8593a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2024-01-07 21:47:14Functions:125422.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/index.html b/mrs_uav_state_estimators/src/estimators/heading/index.html new file mode 100644 index 0000000000..98ea56e1c5 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8846618.9 %
Date:2024-01-07 21:47:14Functions:125422.2 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
hdg_generic.cpp +
0.0%
+ 		0.0 %0 / 3350.0 %0 / 31
hdg_passthrough.cpp +
67.2%67.2%
+ 		67.2 %88 / 13152.2 %12 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html new file mode 100644 index 0000000000..901ecc6ed7 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
<unnamed>58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html new file mode 100644 index 0000000000..e283f41211 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
<unnamed>58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-detail.html b/mrs_uav_state_estimators/src/estimators/lateral/index-detail.html new file mode 100644 index 0000000000..841a9d0b60 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
<unnamed>58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html new file mode 100644 index 0000000000..0902e3a283 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html new file mode 100644 index 0000000000..487da9ef87 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/index.html b/mrs_uav_state_estimators/src/estimators/lateral/index.html new file mode 100644 index 0000000000..cc78f858f0 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
lat_generic.cpp +
58.3%58.3%
+ 		58.3 %236 / 40583.3 %25 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..6f15e20176 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)60
mrs_uav_state_estimators::LatGeneric::isConverged()60
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)60
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)232
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)232
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const364
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const542
mrs_uav_state_estimators::LatGeneric::start()1543
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) const11083
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)102166
mrs_uav_state_estimators::LatGeneric::setDt(double const&)102177
mrs_uav_state_estimators::LatGeneric::getStates() const102179
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const102182
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&)113550
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) const113579
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)113581
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)118863
mrs_uav_state_estimators::LatGeneric::generateA()204388
mrs_uav_state_estimators::LatGeneric::generateB()204412
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const224310
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const224369
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const448713
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const448791
mrs_uav_state_estimators::LatGeneric::getState(int const&) const889175
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const889434
+
+
+ + + +
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..ed269b26dc --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::LatGeneric::initialize(ros::NodeHandle&, std::shared_ptr<mrs_uav_managers::estimation_manager::CommonHandlers_t> const&, std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> const&)60
mrs_uav_state_estimators::LatGeneric::isConverged()60
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)118863
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&)113550
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)113581
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)102166
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)60
mrs_uav_state_estimators::LatGeneric::setCovarianceMatrix(Eigen::Matrix<double, 6, 6, 0, 6, 6> const&)0
mrs_uav_state_estimators::LatGeneric::pause()0
mrs_uav_state_estimators::LatGeneric::reset()0
mrs_uav_state_estimators::LatGeneric::setDt(double const&)102177
mrs_uav_state_estimators::LatGeneric::start()1543
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)232
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)232
mrs_uav_state_estimators::LatGeneric::generateA()204388
mrs_uav_state_estimators::LatGeneric::generateB()204412
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const542
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const448713
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const448791
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const224310
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const224369
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const364
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const102182
mrs_uav_state_estimators::LatGeneric::getState(int const&) const889175
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const889434
mrs_uav_state_estimators::LatGeneric::getStates() const102179
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) const113579
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) const11083
+
+
+ + + +
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..e5de3b778d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html @@ -0,0 +1,838 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/lateral - lat_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:23640558.3 %
Date:2024-01-07 21:47:14Functions:253083.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c3b67fa41f0c650fd83d52174734d752690228e0 GIT binary patch literal 2974 zcmV;P3t{w$P)_0;S;5*sS;5}G*?*N6>jy}HP-&t2xe=DV_f0De-3RF5u(g9?kIKUGj@*F^*|@c>nOH z0@@!PhI8fb__BV89V0NZ!^c33ff_VN8({0zE3I1t*e)55ajC$3_*XN>1I|G8C2q`` zsYs3mkR9E@NAWgumod z^swKWqwcy+*>;EgnS!t}d#$5a^cvX_IsXC}6JX+&0$6iX1oR1`@a*={T|jRoCLnEB z+!sLOK#$hs2R0<%3yPK9H4T?BTIYzwY*xi=z=j)04jzCt2T(m)WK#~Ml=wDomhV1V zULc4SYJ*mdPg*ncXhSKUa)@1h`4MH%(Nx8B=#!d^YWbLc9Gm5qx0iSLXk6-LdRTI^ z%ScWtd(>S(>c-2A8cL?x1a~2jUUU%aJcWue(9@rzaZJ-}jy^COK`fn5M7o%iDRb;w zT&txpD_Likeq)%$jH=Kk>x@mc4nTsj_PzKP zh=dU$hiL1;^gq!)dcn$=hf!`vBM$wIJg~Q1E;W*i&WEX~;gTvT&ciBmG(P%}aFA@--dE|cVDR(*!;-lt zK#M${o;N#I#`RaQ4#Zt9o2A$XMglbOaWO9TJH+VpggK^SVsV%Y9W`LCYC;DMk1;%) zi^N+%*N+WzsS@xBMXFNMYzN6m`1b)*T#>$gVwbOzE-l(J$77hBQufig?u&R^04*6L zlc0qu8pW(-j&BsfH_0n4fcyrX>Dh38U2=qpR|dA04#ozhZ{@K8R8pTt{`?w-+ii~_ zVHo`$yz?Z#_}G|s?PlG_{rEnCv4av2zY{>q?@y%)tb8~d~D?#Su=@kc9DE+hZOz(csuq9wGMZD8FC2 zuJ$Y!3?C!qHO~oqAZ-+|Z?)}d%JGsGGD`C&3_sws#2RH!|H%VdIJATYDI3bAoNnw- z?=7tBT#^TZ>RfsQ{{<0yv}K1i>!eD67!PDI0T@>#@zK{zKhnVt9M1rU8hjXI`1Oyn z2T|RN*wHep(Z_XjbY7k6XA?uROWn?~yV9Q}W{O>Sx2QTwk};&ikW5qYCJp&6te#tP zp2t1l3zY6FmN3)eQ26Nb6kSo*Pt0*D&D%HhT^1|$c^_4)B{0!?4oU5$EP#xCWK?mg zQdbJbfU?`QvsAll^7S?pT_7(-@Jrs_G|kde8c87@Z%sWIdrY+oNQT5mvQer1Jw`y= zNNKnra<@5Er$`%3Dpyfr{3BU_k|-@LxWqnclSCN1htu?4VF0{BUE!231XFWcT*1b+ zG>>y7nEP}vdoJBcFe>mD;4C4j0H+&tI~CNo#^?vYcHF90;nD5?=GkL|vyUW!w|f=5 z)>~NWbZd09c5RX&b+wK*ecHokAQ>TOh%vXGB`3Fs7j&jr0YMdv?TUPp_6FyHaGs*B zOS3`bWQ_LKm>C97-Ag@F1GzmbyH}KP2;s7v(JVa4f%`KC*N9tBa#l);tTV>u|2$E$jfBy)_$vrhJaQxpVlJX<){ zgJh7&i#jwA-6u7${58e+Kgrpb+5_zw{sv-ndLWq!DN>-L2DB%o5BtsRUM=9(-@I~; z6z~b%Qf$_>m9i*i?*VRQOUljS@vTZ{TC`=3$1oM+*+=J1k;^sryA{{7bM_o__C^u* z=5SiiQW)lVsm=^nAX8CcCjvw&6`GXSsrap^W7S31=IoW)@JQv%1o%;ge@Q{?2aECR zFIkyNZlC`|G5*(8a-RbGKV0%f&FSfih;saNh6thIj$ln&`zR>vNA2V3g+*H<2ad0I zr5k^}E^M|G7J(QD5;$jaVttRj_+@)adlTP`2)T?=EJ$C*9z=D|ND)m{JgHxEYN#8p zGwt)jd0(1zarm6*yLpOU#-jlK2-Ue}4vn9F>?XwTKQs`k0Jm5qL4}c0vP+q2;R+_# zO2(&a!?(P(Y3HwUrPp`X#-5&CnDjM&U4n6tE@Z7Dj{uiYu7kX@C#pCEXrCiKa)Mvu z+sxRQE51nGgW;O4Kg zy#~M%405L`hXL7{)qw9YT#LCBhG!fX%<&jjpKJ8|j8iX7xS$MZ9wXIet8aEe$(Ot# z-C|g7iHUpW0jRa(WxyAx4v2my-Nig~li>xvmdU$M7)TPxBR} z^KO69qvE(PJqGwZulM!QZ?NE%6xpdZ4{?sw(O-6s=-Wh$Z{<>)S1Bz$pcOF5#{ zPPGwkG0pK9!2A!qrHlB9F}+zd#a%@gP$ zF9i?*Ewi)(et|n``;{9SDlM`xkQy + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-01-07 21:47:14Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()4
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()4
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()4
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().24
+
+
+ + + +
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..891771c459 --- /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-01-07 21:47:14Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()4
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()4
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()4
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().24
+
+
+ + + +
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..91e7ec1191 --- /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-01-07 21:47:14Functions: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           4 :   GpsBaro() : StateGeneric(estimator_name, is_core_plugin) {
+      15           4 :   }
+      16             : 
+      17           8 :   ~GpsBaro(void) {
+      18           8 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_baro
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25           4 : 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-01-07 21:47:14Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()52
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()52
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()52
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().252
+
+
+ + + +
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..8edca970cc --- /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-01-07 21:47:14Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()52
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()52
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()52
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().252
+
+
+ + + +
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..d808a2b079 --- /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-01-07 21:47:14Functions: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          52 :   GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15          52 :   }
+      16             : 
+      17         104 :   ~GpsGarmin(void) {
+      18         104 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          52 : 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..47055fcabc --- /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-01-07 21:47:14Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html new file mode 100644 index 0000000000..4682193709 --- /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-01-07 21:47:14Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-detail.html b/mrs_uav_state_estimators/src/estimators/state/index-detail.html new file mode 100644 index 0000000000..ac1a6f2deb --- /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-01-07 21:47:14Functions: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..87765d5bb4 --- /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-01-07 21:47:14Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html new file mode 100644 index 0000000000..5e18463471 --- /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-01-07 21:47:14Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_baro.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/index.html b/mrs_uav_state_estimators/src/estimators/state/index.html new file mode 100644 index 0000000000..813032541b --- /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-01-07 21:47:14Functions: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..474e9aba14 --- /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-01-07 21:47:14Functions: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&)1028
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)1043
+
+
+ + + +
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..bf7d37fcde --- /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-01-07 21:47:14Functions: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&)1028
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckHealth(ros::TimerEvent const&)1043
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..04fad4ce31 --- /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-01-07 21:47:14Functions: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        1028 : void Passthrough::timerUpdate(const ros::TimerEvent &event) {
+     140             : 
+     141             : 
+     142        1028 :   if (!isInitialized()) {
+     143           0 :     return;
+     144             :   }
+     145             : 
+     146        1028 :   const ros::Time time_now = ros::Time::now();
+     147             : 
+     148        2056 :   nav_msgs::OdometryConstPtr msg = sh_passthrough_odom_.getMsg();
+     149             : 
+     150        1028 :   if (first_iter_) {
+     151           1 :     prev_msg_   = msg;
+     152           1 :     first_iter_ = false;
+     153             :   }
+     154             : 
+     155        2056 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     156             : 
+     157        1028 :   uav_state.header.stamp = time_now;
+     158             : 
+     159        1028 :   uav_state.pose.position    = msg->pose.pose.position;
+     160        1028 :   uav_state.pose.orientation = msg->pose.pose.orientation;
+     161             : 
+     162        1028 :   uav_state.velocity.linear  = Support::rotateVector(msg->twist.twist.linear, msg->pose.pose.orientation);
+     163        1028 :   uav_state.velocity.angular = msg->twist.twist.angular;
+     164             : 
+     165        2056 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     166             : 
+     167        2056 :   nav_msgs::Odometry innovation = innovation_init_;
+     168        1028 :   innovation.header.stamp       = time_now;
+     169             : 
+     170        1028 :   innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x;
+     171        1028 :   innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y;
+     172        1028 :   innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z;
+     173             : 
+     174        2056 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     175        1028 :   pose_covariance.header.stamp  = time_now;
+     176        1028 :   twist_covariance.header.stamp = time_now;
+     177             : 
+     178        1028 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     179        1028 :   pose_covariance.values.resize(n_states * n_states);
+     180        1028 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     181        1028 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     182        1028 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     183             : 
+     184        1028 :   twist_covariance.values.resize(n_states * n_states);
+     185        1028 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     186        1028 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     187        1028 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     188             : 
+     189        1028 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     190        1028 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     191        1028 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     192        1028 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     193        1028 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     194             : 
+     195        1028 :   publishUavState();
+     196        1028 :   publishOdom();
+     197        1028 :   publishCovariance();
+     198        1028 :   publishInnovation();
+     199        1028 :   publishDiagnostics();
+     200             : 
+     201        1028 :   prev_msg_ = msg;
+     202             : }
+     203             : /*//}*/
+     204             : 
+     205             : /*//{ timerCheckHealth() */
+     206        1043 : void Passthrough::timerCheckHealth(const ros::TimerEvent &event) {
+     207             : 
+     208        1043 :   if (!isInitialized()) {
+     209           0 :     return;
+     210             :   }
+     211             : 
+     212        1043 :   if (isInState(INITIALIZED_STATE)) {
+     213             : 
+     214          15 :     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          14 :       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          14 :       return;
+     220             :     }
+     221             :   }
+     222             : 
+     223             : 
+     224        1029 :   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(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..952c0015116d8270a63eb96783a0d220ca31918b GIT binary patch literal 1098 zcmV-Q1hxB#P)*X4RlZ_&k1 zGc8g-$K^HUkd6ySr`IJT(GA`oh&Ewanxd7GDkrHx-G6~5!K6tj00plC`J#>X(!ZLw zsSrNL%aJa@XZ^Q?PujuVp%cO3@+GSU5y2#$(u9x;WJJZdhi^4WcLEHi8H$V!$AZIx zBSVXJQiG)>9c6-ZK%ZUsZ5o*YV-}QMC1Y-FAk!4-jEsQ>a6rd7S^`an%hWO0kKVE6 zS`au|U>dp{m>OOLg5_9{or`V)foqIrng<6wkV%-Mq^kL&STF5ZmxcN-aM{yel#OvI zUQAoC9JxhGq+1-&znr%1bad?dStJZR>%e1U(SVL+x%(5*1-X&v<0zZlC7Mr3eY6QV zN4?!l)E<{bc?XaH(L#h6$Ruj}a0>$(K*0n;2{^3d{a?qk1_TU5SbEdEfUmS%^}Vp8 z`A7>Co#mlH0)I%_UThhskjJZ$nsS zw?!e`?S2%>!HN1hAHtSDKj`+DS=QyAxayrz2Rq7Ed@kFzh2bzn9G1oR4cM8e*X zxPGDmow`7y|8VV$K7*BpiKl%_ABs-lOj4(CIX8q&b9F=kW`F(cY)KmR0B zZ~SaHvj*I#>3!g!j-Fpyg;HR}J(NaI{Rpl&f&_c{NJ&tkaFPx;Kgs12z#9gjl6v$7 zu*2$%YHPMb@ QfB*mh07*qoM6N<$f+&#(bpQYW literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html new file mode 100644 index 0000000000..d7b24e6491 --- /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-01-07 21:47:14Functions: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..570a1f61b5 --- /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-01-07 21:47:14Functions: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..cffdc6bdce --- /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-01-07 21:47:14Functions: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..5bfbb5d443 --- /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-01-07 21:47:14Functions: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..b969442744 --- /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-01-07 21:47:14Functions: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..c06b3dec02 --- /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-01-07 21:47:14Functions: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..7a8496d4e7 --- /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-01-07 21:47:14Functions: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&)60
mrs_uav_state_estimators::StateGeneric::start()5593
mrs_uav_state_estimators::StateGeneric::updateUavState()112247
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)119069
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)119069
mrs_uav_state_estimators::StateGeneric::getHeading() const182496
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()() const182497
+
+
+ + + +
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..a2f9c959cf --- /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-01-07 21:47:14Functions: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&)60
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&)119069
mrs_uav_state_estimators::StateGeneric::updateUavState()112247
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)119069
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()5593
mrs_uav_state_estimators::StateGeneric::getHeading() const182496
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()() const182497
+
+
+ + + +
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..1615dd4537 --- /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-01-07 21:47:14Functions: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          60 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13          60 :   ch_ = ch;
+      14          60 :   ph_ = ph;
+      15             : 
+      16         120 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18          60 :   if (is_core_plugin_) {
+      19             : 
+      20          60 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21          60 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24          60 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          60 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27          60 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28          60 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29          60 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31          60 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32          60 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36         120 :   std::string topic_orientation;
+      37          60 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38          60 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39         120 :   std::string topic_angular_velocity;
+      40          60 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41          60 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43          60 :   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          60 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51          60 :   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          60 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56         120 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57          60 :   shopts.nh                 = nh;
+      58          60 :   shopts.node_name          = getPrintName();
+      59          60 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60          60 :   shopts.threadsafe         = true;
+      61          60 :   shopts.autostart          = true;
+      62          60 :   shopts.queue_size         = 10;
+      63          60 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65          60 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66          60 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69          60 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70          60 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72          60 :   if (ch_->debug_topics.state) {
+      73          60 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75          60 :   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          60 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82          60 :   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         120 :   std::vector<double> max_altitudes;
+      88             : 
+      89          60 :   if (is_hdg_passthrough_) {
+      90          60 :     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          60 :   est_hdg_->initialize(nh, ch_, ph_);
+      95          60 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97      182557 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98          60 :   est_lat_->initialize(nh, ch_, ph_);
+      99          60 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101          60 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102          60 :   est_alt_->initialize(nh, ch_, ph_);
+     103          60 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105          60 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108          60 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109          60 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111          60 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112          60 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113          60 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115          60 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116          60 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117          60 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121          60 :   if (changeState(INITIALIZED_STATE)) {
+     122          60 :     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          60 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130        5593 : bool StateGeneric::start(void) {
+     131             : 
+     132        5593 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136        5593 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137        4050 :       est_lat_start_successful = true;
+     138             :     } else {
+     139        1543 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142        5593 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143        1181 :       est_alt_start_successful = true;
+     144             :     } else {
+     145        4412 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148        5593 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149        5493 :       timer_pub_attitude_.start();
+     150        5493 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152         100 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155        5593 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157          60 :       changeState(STARTED_STATE);
+     158          60 :       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        5533 :   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      119069 : void StateGeneric::timerUpdate(const ros::TimerEvent &event) {
+     209             : 
+     210             : 
+     211      119069 :   if (!isInitialized()) {
+     212        6795 :     return;
+     213             :   }
+     214             : 
+     215      237266 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     216             : 
+     217      118623 :   switch (getCurrentSmState()) {
+     218             : 
+     219           0 :     case UNINITIALIZED_STATE: {
+     220           0 :       break;
+     221             :     }
+     222         787 :     case INITIALIZED_STATE: {
+     223             : 
+     224         787 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     225          60 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     226          60 :           changeState(READY_STATE);
+     227          60 :           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         727 :         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         727 :         return;
+     235             :       }
+     236             : 
+     237          60 :       break;
+     238             :     }
+     239             : 
+     240        5563 :     case READY_STATE: {
+     241        5563 :       break;
+     242             :     }
+     243             : 
+     244          77 :     case STARTED_STATE: {
+     245             : 
+     246          77 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     247             : 
+     248          77 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     249          60 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     250          60 :         changeState(RUNNING_STATE);
+     251             :       } else {
+     252          17 :         return;
+     253             :       }
+     254          60 :       break;
+     255             :     }
+     256             : 
+     257      112209 :     case RUNNING_STATE: {
+     258      112209 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     259           0 :         changeState(ERROR_STATE);
+     260             :       }
+     261      112202 :       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      117877 :   if (!isRunning() && !isStarted()) {
+     277        5622 :     return;
+     278             :   }
+     279             : 
+     280      112242 :   updateUavState();
+     281             : 
+     282      112225 :   publishUavState();
+     283      112275 :   publishOdom();
+     284      112276 :   publishCovariance();
+     285      112276 :   publishInnovation();
+     286      112276 :   publishDiagnostics();
+     287             : }
+     288             : /*//}*/
+     289             : 
+     290             : /*//{ timerCheckHealth() */
+     291           0 : void StateGeneric::timerCheckHealth(const ros::TimerEvent &event) {
+     292             : 
+     293           0 :   if (!isInitialized()) {
+     294           0 :     return;
+     295             :   }
+     296             : 
+     297           0 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299           0 :   switch (getCurrentSmState()) {
+     300             : 
+     301           0 :     case UNINITIALIZED_STATE: {
+     302           0 :       break;
+     303             :     }
+     304           0 :     case INITIALIZED_STATE: {
+     305             : 
+     306           0 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     307           0 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     308           0 :           changeState(READY_STATE);
+     309           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     310             :         } else {
+     311           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     312           0 :           return;
+     313             :         }
+     314             :       } else {
+     315           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_hw_api_orient_.topicName().c_str());
+     316           0 :         return;
+     317             :       }
+     318             : 
+     319           0 :       break;
+     320             :     }
+     321             : 
+     322           0 :     case READY_STATE: {
+     323           0 :       break;
+     324             :     }
+     325             : 
+     326           0 :     case STARTED_STATE: {
+     327             : 
+     328           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     329             : 
+     330           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     331           0 :         changeState(ERROR_STATE);
+     332             :       }
+     333             : 
+     334           0 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     335           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     336           0 :         changeState(RUNNING_STATE);
+     337             :       } else {
+     338           0 :         return;
+     339             :       }
+     340           0 :       break;
+     341             :     }
+     342             : 
+     343           0 :     case RUNNING_STATE: {
+     344           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     345           0 :         changeState(ERROR_STATE);
+     346             :       }
+     347           0 :       break;
+     348             :     }
+     349             : 
+     350           0 :     case STOPPED_STATE: {
+     351           0 :       break;
+     352             :     }
+     353             : 
+     354           0 :     case ERROR_STATE: {
+     355           0 :       if (est_lat_->isReady() && est_alt_->isReady() && est_hdg_->isReady()) {
+     356           0 :         changeState(READY_STATE);
+     357             :       }
+     358           0 :       break;
+     359             :     }
+     360             :   }
+     361             : }
+     362             : /*//}*/
+     363             : 
+     364             : /* timerPubAttitude() //{*/
+     365      119069 : void StateGeneric::timerPubAttitude(const ros::TimerEvent &event) {
+     366             : 
+     367      119069 :   if (!isInitialized()) {
+     368        1326 :     return;
+     369             :   }
+     370             : 
+     371      237260 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     372             : 
+     373      118603 :   if (!sh_hw_api_orient_.hasMsg()) {
+     374         154 :     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         154 :     return;
+     376             :   }
+     377             : 
+     378      118472 :   if (!est_hdg_->isRunning() && !isError()) {
+     379         743 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     380         743 :     return;
+     381             :   }
+     382             : 
+     383      117710 :   scope_timer.checkpoint("checks");
+     384             : 
+     385      117724 :   const ros::Time time_now = ros::Time::now();
+     386             : 
+     387      117741 :   geometry_msgs::QuaternionStamped att;
+     388      117725 :   att.header.stamp    = time_now;
+     389      117725 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     390             : 
+     391             :   double hdg;
+     392      117727 :   if (isError()) {
+     393           0 :     hdg = est_hdg_->getLastValidHdg();
+     394             :   } else {
+     395      117693 :     hdg = est_hdg_->getState(POSITION);
+     396             :   }
+     397             : 
+     398      117696 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     399      117579 :   if (res) {
+     400      117666 :     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      117548 :   scope_timer.checkpoint("rotate");
+     407             : 
+     408      117658 :   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      117642 :   scope_timer.checkpoint("nan check");
+     414             : 
+     415      117528 :   ph_attitude_.publish(att);
+     416      117744 :   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      112247 : void StateGeneric::updateUavState() {
+     432             : 
+     433      112247 :   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      112249 :   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      224526 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     443             : 
+     444      112263 :   const ros::Time time_now = ros::Time::now();
+     445             : 
+     446      112276 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     447      112270 :   uav_state.header.stamp       = time_now;
+     448             : 
+     449             :   // do not rotate orientation if passthrough hdg
+     450      112270 :   if (est_hdg_name_ == "hdg_passthrough") {
+     451           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     452             :   } else {
+     453             :     double hdg;
+     454      112269 :     if (isError()) {
+     455           0 :       hdg = est_hdg_->getLastValidHdg();
+     456             :     } else {
+     457      112261 :       hdg = est_hdg_->getState(POSITION);
+     458             :     }
+     459             : 
+     460      112244 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     461      112174 :     if (res) {
+     462      112221 :       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      112164 :   scope_timer.checkpoint("rotate orientation");
+     470             : 
+     471      112239 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     472             : 
+     473      112200 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     474      112244 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     475      112234 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     476             : 
+     477      112211 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     478      112227 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     479      112250 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     480             : 
+     481      112219 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     482      112231 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     483      112238 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     484             : 
+     485      112232 :   scope_timer.checkpoint("fill uav state");
+     486             : 
+     487      224468 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     488      112266 :   scope_timer.checkpoint("uav state to odom");
+     489             : 
+     490      224423 :   nav_msgs::Odometry innovation = innovation_init_;
+     491      112252 :   innovation.header.stamp       = time_now;
+     492             : 
+     493      112252 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     494      112156 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     495      112186 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     496             : 
+     497      112171 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     498             : 
+     499      112261 :   scope_timer.checkpoint("innovation");
+     500             : 
+     501      224394 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     502      112124 :   pose_covariance.header.stamp  = time_now;
+     503      112124 :   twist_covariance.header.stamp = time_now;
+     504             : 
+     505      112124 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     506      112124 :   pose_covariance.values.resize(n_states * n_states);
+     507      112167 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     508      112093 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     509      112162 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     510             : 
+     511      112095 :   twist_covariance.values.resize(n_states * n_states);
+     512      112161 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     513      112222 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     514      112231 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     515             : 
+     516      112209 :   scope_timer.checkpoint("covariance");
+     517             : 
+     518      112170 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     519      112176 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     520      112160 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     521      112244 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     522      112164 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ getHeading() */
+     527      182496 : std::optional<double> StateGeneric::getHeading() const {
+     528      182496 :   if (!est_hdg_->isRunning()) {
+     529         101 :     return {};
+     530             :   }
+     531      182385 :   return est_hdg_->getState(POSITION);
+     532             : }
+     533             : /*//}*/
+     534             : 
+     535             : /*//{ setUavState() */
+     536           0 : bool StateGeneric::setUavState(const mrs_msgs::UavState &uav_state) {
+     537             : 
+     538           0 :   if (!isInState(STOPPED_STATE)) {
+     539           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     540           0 :     return false;
+     541             :   }
+     542             : 
+     543           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     544           0 :   return false;
+     545             : }
+     546             : /*//}*/
+     547             : 
+     548             : }  // namespace mrs_uav_state_estimators
+     549             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..200fa9f980 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html @@ -0,0 +1,158 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2900f2b854c8fdbf8391333f47e3b1b1066a5d06 GIT binary patch literal 1971 zcmV;k2Tb^hP)RX!#_;9zBJ-wZR?-u9;XlY|Hk&&Tum9N+(j ztHvAOj#M7a7{|+)BRq}>OP5m~oNnIKE4b%b&FRqos{JR!1dd^vwVw5*fH-N&{yblS zF{npf#N#yAhEi7HC?KH2oiscI%A2_~{h-}4St;CW70}bxmtmYwdN@FP$H*p^>_CGt zufkaE)fh9F?Fx)mTOVd^m-_gwjsV~OsZ{c-gPRi4vk(m+TRVjlT!**Dj;l`v07kW7 z!e#rvysfr|_wKrVKmaQy>}BiJCb8Oa^}G{J$im~1l0()TM^2i@kBljq!c|uHiLJ{> zPYLrN*C0(S4Cg%WJyg4LwmUF7KqpuMFcw?{)DELSJ0)EO)Tl5!($qLQ0D^;%F1ZAA ziW(qFnmEN+cZz9}=_<}+nZYI=qaY@qEY)(jENihwvm&gi-Gu&Q0$J6&~ zku3v`!ElVLWKrrA6^uOrEB%r%aP&h=y0LWXet(Mqsv@0W^&1F%=#m&6EMtoAHXg9+gweS^*rS zn32~glV52 zadp~%l(on8cyTYqQU)+~S)4g76l2u%9XwV8DK?@7q&nu7(yYgKdS0`+s$z_pNk}$` z)R3oUUM zzNom`gywqnT)s6m__udx>1O;l6q*&xXKr7CfDIVSu%`ga*%iK#)2plCmQy9%QCHzu zb^3+pddwMYDztSKfce$6S>SpY!e)}@^>qkf1*uVt_pOi^>4Fhkgo&D;Jl+S5*sTnnZvWBjreQl3SA^C3tLu-K4Y@mcsIRm$x=)~h{? zw8{EJm{u!u3K$~^Qsj3!zZcV?h--2);_<6gb!O*``^s_602X5p5>ZTtS!jO4aqsdT z^CRlHoyVqC+^?Gut-|lTUr4vkm2l@yENb%V*ORBVX#G|9J|aB|O}Qa$0cy(x?LsPl zV(~8Zu#HaZ^&2ZzCHh)tmUXMt-FwY zq3RA-hSGJ<%ggJ|rc00Mnph>9m~!elN)>Ya^n28!Z7BFqJ#@f;GVoV)G;3R>;*O>i zUK%#0Z8g^;9CjV;kG(|uA0O^O!^3Tqw3qWz|HQVPqt{$)HK!G+d`km#Gq^TO7xwPh zIqiNdcL#v>BciD=ix2n9+Ci8uTZ_7tT#Df=o)DY-;ioJ{l=QF#h2uz*j}YmM3%1tf zHC4PHH+v4#nGmd`;Bs#Q%e{Rej+A=?*bTGy*pYIt+ZKd+x#!e_?a*M~l!`w5+=l_n zJqK8y-1pw-<|B9h(d!W!Lss1xS2~F?c1%SvUIM5o>2p$NX{#;TTm@(kj9S2RJm%7e zC4VKLmA1BPoGSt6(>x~A z;JOERNhgudVGMl5rBV!7Fkw!OpL@8g;I`mlA4fp;AM3(H#&XYA$^dR)TF;TGdH|lZ z3{mD>1Kh!O!D)Kx0r-J1*E;St$nz^^8PILl%~Svx9?6wFFi8TO$-L1OK>EChujqV~ zSJ&J6I%VBS%&N+kv-bPf0bAqGK}xy+dBAT_cx$VP?wYdK)tA@Uh>wVtnpc!icUM!T4Yo>L2r{{ZriD^SDq$1eZ?002ovPDHLk FV1hK7w8;Pf literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..2829557e55 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-01-07 21:47:14Functions: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..1e0d1c5c1e --- /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-01-07 21:47:14Functions: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..12e94e0eda --- /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-01-07 21:47:14Functions: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..7241937911 --- /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-01-07 21:47:14Functions: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..8129480952 --- /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-01-07 21:47:14Functions: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..64f1de9326 --- /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-01-07 21:47:14Functions: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..574f16a18e --- /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-01-07 21:47:14Functions: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&)3
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()13
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)182
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)64638
+
+
+ + + +
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..cecc5bf068 --- /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-01-07 21:47:14Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()13
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>)57
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&)182
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
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&)3
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&)64638
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..df9786aa57 --- /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-01-07 21:47:14Functions: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          57 : 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          57 :   this->common_handlers_  = common_handlers;
+     136          57 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138          57 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140          57 :   nh_ = nh;
+     141             : 
+     142          57 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150         114 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152          57 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154          57 :   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          57 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162          57 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164         114 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182          57 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184          57 :   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          57 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195          57 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196          57 :   shopts.nh              = nh_;
+     197          57 :   shopts.node_name       = "JoyTracker";
+     198          57 :   shopts.queue_size      = 1;
+     199          57 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201          57 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205          57 :   last_update = ros::Time(0);
+     206             : 
+     207          57 :   is_initialized_ = true;
+     208             : 
+     209          57 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211          57 :   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          13 : void JoyTracker::deactivate(void) {
+     277             : 
+     278          13 :   is_active_ = false;
+     279             : 
+     280          13 :   ROS_INFO("[JoyTracker]: deactivated");
+     281          13 : }
+     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       64638 : 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      193914 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300      193914 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303       64638 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305       64638 :     uav_state_ = uav_state;
+     306             : 
+     307       64638 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310       64638 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312       64638 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315       64638 :   if (!is_active_) {
+     316       64638 :     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         169 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389         338 :   std_srvs::SetBoolResponse res;
+     390         338 :   std::stringstream         ss;
+     391             : 
+     392         169 :   if (cmd->data != callbacks_enabled_) {
+     393             : 
+     394          11 :     callbacks_enabled_ = cmd->data;
+     395             : 
+     396          11 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     397          11 :     ROS_INFO_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     398             : 
+     399             :   } else {
+     400             : 
+     401         158 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402         158 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405         169 :   res.message = ss.str();
+     406         169 :   res.success = true;
+     407             : 
+     408         338 :   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         182 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467         182 :   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           3 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr JoyTracker::setTrajectoryReference([
+     493             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     494           3 :   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          57 : 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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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 +
72.1%72.1%
+ 		72.1 %427 / 59267.7 %21 / 31
<unnamed>72.1 %427 / 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..4bf3770e3c --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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 +
72.1%72.1%
+ 		72.1 %427 / 59267.7 %21 / 31
<unnamed>72.1 %427 / 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..8300dc96ce --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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 +
72.1%72.1%
+ 		72.1 %427 / 59267.7 %21 / 31
<unnamed>72.1 %427 / 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..44245a0e8e --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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 +
72.1%72.1%
+ 		72.1 %427 / 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..212345cc4b --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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 +
72.1%72.1%
+ 		72.1 %427 / 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..8c2c28d897 --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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 +
72.1%72.1%
+ 		72.1 %427 / 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..71ba5debec --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions: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> >&)3
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)5
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)10
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)23
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()36
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)75
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)93
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)113
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)182
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()188
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()188
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()1403
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()2240
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()10327
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()12567
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)14027
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)64638
+
+
+ + + +
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..7c30e9e5eb --- /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:42759272.1 %
Date:2024-01-07 21:47:14Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()36
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>)57
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)75
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> >&)3
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> >&)5
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)182
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()12567
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)10
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()10327
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()2240
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()188
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)113
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()188
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)93
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)3
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&)64638
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)23
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()1403
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)14027
+
+
+ + + +
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..11a9d3fbad --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html @@ -0,0 +1,1636 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:42759272.1 %
Date:2024-01-07 21:47:14Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..51077bb7cab327b6b03edeef409fb81de0a9e850 GIT binary patch literal 4772 zcmV;V5?k$wP)1 zRCt{2U5jDsDh!P`uvf4*a8`&naQ9#3#(X6qK>V7zx7_#IBs~#G2qA>Pk7ZhxWq$vA z^eS(9is+If>48~jan-iWJt-DcUDC!@_A$i-$F}w0#j5)rCA3;ij!*O)Q_+l zgNkS`nq#=`%Jf4sJ>^HN7+6^KJY{v#c1*vz2lR|ppWJxqUNfpXNqs=olh#OGcaIsw z(#r!C(*lb9xqgWCPpqk8Q!6&e;&D<6>?hB;FS&=Y6m~33Qzx2+q#NEw;V1a6u#Jl~ zXrAa*!;FDa;rO;Va#QHwEbt4-B4@@?no#wOb492+mKlmHu)(-69L^8g&}o5P1DeLw zPWlb;4qj$~OR@H_6fFhE%^a!Pf{~Y;0Lt8HQ5)xwcZqn^TM=-Q0xzThIGZ-M_Ka(s zAzMOFT0f{@p1asomJ6uj2$=qQVRL+C65w1g9ZRcD6w@hr;4JSwUG=ocdQMSAbC0Qw zeis1JWD%teLkoC(V zH8CETtJ=>J%1_Ax--t9&T<`>yXu`sH8g2FaE;J65S&lems$d2;(zv~0J1KCM0v9oG zjsOdop&_XV!5{`0s~G8RhYkdurDs_H)ZHQg&Iw>;8Q3B{2sa&mbwk0u@CJ`DM4-+k z>F_xTc9SLann4TQhm~Ag?+yN-4d#F>QyvI$P-Nzd7|4G?RRBM)6dY|M&>kmA@KM7Q zxAd}BbX`bf)6m9jw8s6>v(&g+!OW0(t6AVqvG~n6#H{}c#T7Rp+r$BTniJp`*??Lx z+TeG$jEd)Td*ZU&7aRQ3VseO>+#PUdfWl%ZZiw}eUKz!A%QzB^ zZH$YhoZ^FbalyFsdgi;y1qpz<0-%thxR^mRFV4(QW`3XQ1`UuSRdw`vZO>3TUYSZL z@=e<)@b&sa<+%PkzRveQU0%9q;QO!VD_tJ19Kr&NyKwOVrWsBlO^wr;8} zHA8FKKmz+H9N3T7mr{mo|2$+JsFgyg*LI{p_)IoQ87-#Bjt^~86j)gG4(Ez46s)%!}LBF_Z1cJtUZB1qdy!F{M-+>?7OXHGNIjXZRgb3jZfEbhga_m1;m5@HMPj z0Q|g~*(h=0~t_$fSM64LvMC$d&rFB-YT8@2}#>oJs6hG+MGhT{! zP27timV)|l9UOMm+-%2i+kl(-W_{JZPdG{rc&A9*&J2oLHSqr!x?I6KLAE}z^9#LDsjI@nEV3BLPC8C$Z zfj;!vD?IDG1<(#%`T!v|h>`9QZd$^#Pd4((1DRNE0Ms9gDDF~Va~t~`sWxK|B;}bA zwvmrNa=R%5tXX)7MSrp(73TH})s7)`nI5k*;*3!TiS279ATW~as@|T&vF2=r z0pcmp0C#9@Dp$jb5mK@K)smUYj=|bcQxbTq!PRYiafx2c5^b+dicReeJJ9tuzL+Y` z4ujgv+WsmJAi3l#qG$&P7@fw;2JzU?BLyviG1HP|%ChviX44kHjNYU+#t{r9|5Ip6sKOA)yK(o8}34B5@JE>C95`v%dtH4KLaR^HKOmxb zx!OrooT&V36wv?Sgm$~KXD%{tnjZohY*Numj6J!G2tFsEy05Qv_xw@YF+h#|>N6L5 zpW=YO5BKVc6Yp- zFEngeSHbdk@TlvZGtC!cfFeZN(|}^2w6E`wNDk*2tUOr$ctRrG;h^u&2a3nOo9R;w ze!$^0#!{S6YaG{N@hFN~(8s}0>H*Zf9s&og3s2`n#paJeo)j!Gy8pLt=1Ti+h1`kb zcKwrMA3^@fvCkgle=o;Q>A;aIJAw@-a4s(Wl`gHqh0h`*FZRZS{MIvP zd>fghFI|mbzt90)^CG~KHYV=JbG>?(jm4?!NMKD0XFQsHv%u2a&GvK>dPLxX(EWPm zOn&8zI>S`xHxzgeiT5}5DU$-N$W2TNeEtKzvUzE{roNlw6BV~fsHvM3K%Yi$fMM?~ zY|norN9Q9zF~!wWOeOG^P?Vj%fIuag*d>*vRf~WE)LNAOwB{t}+bFAgl`|u=6$2jo z&A5d&PwmQB+pI0T**VQ#Yp$5>A*QF+_Rt$PV!(vyX|X*KWv{V4{4qC3(VB?tbqT%4 zjI@*(fZL91GOX`5u6v!8;Knr_(`8)Cubl|$jR5b_Gtl4$R~I+?k3H605gQ&@tQ#I< z-HAQ-_hpym*<-oH&jL`jWv|DhK;1GPC_8kB?%mR+R}la-QPV}Tty#fV3IT3X1=>hFni!NL^qNza`&RlPC8IbhHwA#(YTIA)J> z-AMk7&EP*3P!>ck=ow&0V>jMxkeRRe*^dHdzEp(0J2SgK&ztGO;=axT#S|IM%{Xzm zW2&p}f2@*aCdGAb7?#N~J`BClS376XVF)zP12e<$0*W-ZOpe)M_~r?{wXm$%n0XV?1XX)xQ>ZC1P*z2fji)m!kMt5x_@AgjKH$Ng{r1c zLO|t*EGiOaWz0csx?-nA{k=@!i$A_Ln;Xs49cT6Li2hv8}MxZGZxmh5ez>Ao!}zSxgHs36KKx{oR!T!!~@>yCM#F%(q|3H zhF6ixCDPmrDAF@&M?V-1=ehZdrTpMniZuKnEyJP;&wjxIb(fuSx|439%zHG?=gQM7 zqbPH73r;q5ckN?sWS&aaJE|b&<&$fL-|TWpgZrsR^!A9^6N)lshGZ6Oh-+HRUyE-e zvtN(NyDZcs)kk@%n~<3T_gcz~v}YJ6g>R#DNqRHqquT)yCOIVHXv(;5dJ>EIRay1w z7ueT&dq*!SfrVA0)kgG!Y#Feq>Ng=^pCUjxMSrEcA{&_%w5x!kz3(??aA(8V-ISl) z#-~}i=28a5KzSDmP4n-xkNr*!1zQ>H^=SnT`-hMDA8-hO7flq=Em}=i-e`(%Lcp<) z0Ob_Hoe^jB#`8~FikH31DsvY>?d30umBuGQiw{)^aj$R4(L^VF+Hy2IUR_RIE;o@q z8}sbrDfYz_h4CJJ&xb@Q?(kfD3$%a08K>@*U-<0I5AX$d54-(8SV8U>=vosymRB4E zocTCC77uAxR4KXxrOd3JkdrPqibT(q+Kc1}dxpPvzMPrf!Bn`ZnKY_)^|9*5gGRTo z2LeI2?Dqb2_q+62=UiG!OR_r>q)Bezspuw!sT{NVFW_cTv?3x5K{JJ;)hcB&s(DAE1%`@nYb*Jt%M#^6hZ55}=sAN&BUt z*b6_*tK+b*?X0w5p9Ad0nBNyl>P(E=9CrJyGrLJdk4nUbS8cQq#e8=4Pkp>UAuxH7 z#%CkLTTCL=;(1cfu=zO^aPEq?vU*!OO))YN1i%sd*MdXhbzG+(7O{SoYr)1#G1nrX zl$qYcFdQ@OHw=s&GjU0MTzj^R5Z6Dh1#SHCTq9p5k=Rh>N#%oAX=lp=2c&)f9{V`d zoq6Ki0+gM@xP{wJkLK#rS5i4X&a%#;d8Wu)A=RO2E>WG?;qCY;P(ZYfOPXiohDL`=+Qsb3+~wk*XL&# z5vWP<>-E?z(#9xBImHzm-qXRj-P>^R{yv$~Rtd8Uai>+^4wM=I;V4{X*O7%Q_5^Tj zB6**sl(P$&h4_yM3<0}H4yh;yN!Z30lL)|Gaa?Lr4!d#xy}+V23LY#c-i#jGSwpOphx!&}!Hz&9>2&Vkge+`W$*h$ANtzZdFq2f6e?CvVQEROurS_(dlkfAr!x z&gAAN7WlqXL6=h;dkqRy*nP*J0S$NR9h2kTOfzGd$9Vv&&AQ0zW5dMWcvZBbxRacn zmNtHJhka~XqdnOTK4`movQK6se~i_sN7Y#=_vKDe*8wSmCCtPx(edTKP4JT^oL*_t zhl%N(F0x0T0TCuZyAaw5pRQZ-jBenqC8kA}6`!3n`zOU>AFfP}Z0z&@mZJ+k#~ieh zpfnZuVV>B1+W-D%fw!y+S@*E?tW++jXo0&3_$#Z{Ggq&w{ZyDpsWv^E2NqRr-$swI z7XwX+AB-;-PZ&lBfFrKkg52n_)p(M`$VK!(G=1N$kWZC?lf~tYYyHLz& zjso7}nN7V9bVipcB=xqU3p<{^as%);EPN_Fd+jA1#v$e1nLh_9(uK0%OzDJ#7kq4( z=36GD=F1H<38AqU%`;vi5}>4NVX1uFXveCP7O#ThFPyo0og9wNTsRU_vf~pN3cerz zX08lSN)b+RiP+WWr#Lx9eXFwocuIk;WPpFu@JFX%b}AV<4a=uE3GnmNaGzs?)YNsM y_-m%&c|yBo9q1E^9hRja^( literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..6799a2e138 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-01-07 21:47:14Functions: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..2164135d8f --- /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-01-07 21:47:14Functions: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..d4ea45fcb0 --- /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-01-07 21:47:14Functions: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..c771ae02c8 --- /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-01-07 21:47:14Functions: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..ea0c9f24f1 --- /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-01-07 21:47:14Functions: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..94cd312775 --- /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-01-07 21:47:14Functions: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..f9b3440083 --- /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-01-07 21:47:14Functions: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()180
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&)1680
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2212
+
+
+ + + +
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..c82a9bc7b1 --- /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-01-07 21:47:14Functions: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&)1680
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()180
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2212
+
+
+ + + +
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..becc8f7998 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html @@ -0,0 +1,1359 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-01-07 21:47:14Functions: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        1680 : 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        5040 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     519        5040 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LineTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     520             : 
+     521             :   {
+     522        1680 :     std::scoped_lock lock(mutex_uav_state_);
+     523             : 
+     524        1680 :     uav_state_ = uav_state;
+     525        1680 :     uav_x_     = uav_state_.pose.position.x;
+     526        1680 :     uav_y_     = uav_state_.pose.position.y;
+     527        1680 :     uav_z_     = uav_state_.pose.position.z;
+     528             : 
+     529        1680 :     got_uav_state_ = true;
+     530             :   }
+     531             : 
+     532             :   // up to this part the update() method is evaluated even when the tracker is not active
+     533        1680 :   if (!is_active_) {
+     534         100 :     return {};
+     535             :   }
+     536             : 
+     537        3160 :   mrs_msgs::TrackerCommand tracker_cmd;
+     538             : 
+     539        1580 :   tracker_cmd.header.stamp    = ros::Time::now();
+     540        1580 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     541             : 
+     542             :   {
+     543        1580 :     std::scoped_lock lock(mutex_state_);
+     544             : 
+     545        1580 :     tracker_cmd.position.x = state_x_;
+     546        1580 :     tracker_cmd.position.y = state_y_;
+     547        1580 :     tracker_cmd.position.z = state_z_;
+     548        1580 :     tracker_cmd.heading    = radians::wrap(state_heading_);
+     549             : 
+     550        1580 :     tracker_cmd.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     551        1580 :     tracker_cmd.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     552        1580 :     tracker_cmd.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     553        1580 :     tracker_cmd.heading_rate = speed_heading_;
+     554             : 
+     555        1580 :     tracker_cmd.acceleration.x = 0;
+     556        1580 :     tracker_cmd.acceleration.y = 0;
+     557        1580 :     tracker_cmd.acceleration.z = current_vertical_direction_ * current_vertical_acceleration_;
+     558             : 
+     559        1580 :     tracker_cmd.use_position_vertical   = 1;
+     560        1580 :     tracker_cmd.use_position_horizontal = 1;
+     561        1580 :     tracker_cmd.use_heading             = 1;
+     562        1580 :     tracker_cmd.use_heading_rate        = 1;
+     563        1580 :     tracker_cmd.use_velocity_vertical   = 1;
+     564        1580 :     tracker_cmd.use_velocity_horizontal = 1;
+     565        1580 :     tracker_cmd.use_acceleration        = 1;
+     566             :   }
+     567             : 
+     568        1580 :   return {tracker_cmd};
+     569             : }
+     570             : 
+     571             : //}
+     572             : 
+     573             : /* //{ getStatus() */
+     574             : 
+     575         180 : const mrs_msgs::TrackerStatus LineTracker::getStatus() {
+     576             : 
+     577         180 :   mrs_msgs::TrackerStatus tracker_status;
+     578             : 
+     579         180 :   tracker_status.active            = is_active_;
+     580         180 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     581             : 
+     582         180 :   bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     583             : 
+     584         180 :   tracker_status.have_goal = !idling;
+     585             : 
+     586         180 :   tracker_status.tracking_trajectory = false;
+     587             : 
+     588         180 :   return tracker_status;
+     589             : }
+     590             : 
+     591             : //}
+     592             : 
+     593             : /* //{ enableCallbacks() */
+     594             : 
+     595           0 : const std_srvs::SetBoolResponse::ConstPtr LineTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     596             : 
+     597           0 :   std_srvs::SetBoolResponse res;
+     598           0 :   std::stringstream         ss;
+     599             : 
+     600           0 :   if (cmd->data != callbacks_enabled_) {
+     601             : 
+     602           0 :     callbacks_enabled_ = cmd->data;
+     603             : 
+     604           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     605           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     606             : 
+     607             :   } else {
+     608             : 
+     609           0 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     610           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     611             :   }
+     612             : 
+     613           0 :   res.message = ss.str();
+     614           0 :   res.success = true;
+     615             : 
+     616           0 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     617             : }
+     618             : 
+     619             : //}
+     620             : 
+     621             : /* switchOdometrySource() //{ */
+     622             : 
+     623           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     624             : 
+     625           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     626             : 
+     627           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     628             : 
+     629           0 :   double old_heading  = 0;
+     630           0 :   double new_heading  = 0;
+     631           0 :   bool   got_headings = true;
+     632             : 
+     633             :   try {
+     634           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     635             :   }
+     636           0 :   catch (...) {
+     637           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+     638           0 :     got_headings = false;
+     639             :   }
+     640             : 
+     641             :   try {
+     642           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     643             :   }
+     644           0 :   catch (...) {
+     645           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+     646           0 :     got_headings = false;
+     647             :   }
+     648             : 
+     649           0 :   std_srvs::TriggerResponse res;
+     650             : 
+     651           0 :   if (!got_headings) {
+     652           0 :     res.message = "could not calculate the heading difference";
+     653           0 :     res.success = false;
+     654             : 
+     655           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     656             :   }
+     657             : 
+     658             :   // | --------- recalculate the goal to new coordinates -------- |
+     659             : 
+     660           0 :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     661           0 :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     662           0 :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     663           0 :   double dheading = new_heading - old_heading;
+     664             : 
+     665           0 :   goal_x_ += dx;
+     666           0 :   goal_y_ += dy;
+     667           0 :   goal_z_ += dz;
+     668           0 :   goal_heading_ += dheading;
+     669             : 
+     670             :   // | -------------------- update the state -------------------- |
+     671             : 
+     672           0 :   state_x_ += dx;
+     673           0 :   state_y_ += dy;
+     674           0 :   state_z_ += dz;
+     675           0 :   state_heading_ += dheading;
+     676             : 
+     677           0 :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     678             : 
+     679           0 :   res.message = "odometry source switched";
+     680           0 :   res.success = true;
+     681             : 
+     682           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     683             : }
+     684             : 
+     685             : //}
+     686             : 
+     687             : /* //{ hover() */
+     688             : 
+     689           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     690             : 
+     691           0 :   std_srvs::TriggerResponse res;
+     692             : 
+     693             :   // --------------------------------------------------------------
+     694             :   // |          horizontal initial conditions prediction          |
+     695             :   // --------------------------------------------------------------
+     696             :   {
+     697           0 :     std::scoped_lock lock(mutex_state_, mutex_uav_state_);
+     698             : 
+     699           0 :     current_horizontal_speed_ = sqrt(pow(uav_state_.velocity.linear.x, 2) + pow(uav_state_.velocity.linear.y, 2));
+     700           0 :     current_vertical_speed_   = uav_state_.velocity.linear.z;
+     701           0 :     current_heading_          = atan2(uav_state_.velocity.linear.y, uav_state_.velocity.linear.x);
+     702             :   }
+     703             : 
+     704             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     705             : 
+     706             :   {
+     707           0 :     std::scoped_lock lock(mutex_state_);
+     708             : 
+     709           0 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     710           0 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     711           0 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     712           0 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     713             :   }
+     714             : 
+     715             :   // --------------------------------------------------------------
+     716             :   // |           vertical initial conditions prediction           |
+     717             :   // --------------------------------------------------------------
+     718             : 
+     719             :   double vertical_t_stop, vertical_stop_dist;
+     720             : 
+     721             :   {
+     722           0 :     std::scoped_lock lock(mutex_state_);
+     723             : 
+     724           0 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     725           0 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     726             :   }
+     727             : 
+     728             :   // --------------------------------------------------------------
+     729             :   // |                        set the goal                        |
+     730             :   // --------------------------------------------------------------
+     731             : 
+     732             :   {
+     733           0 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     734             : 
+     735           0 :     goal_x_ = state_x_ + stop_dist_x;
+     736           0 :     goal_y_ = state_y_ + stop_dist_y;
+     737           0 :     goal_z_ = state_z_ + vertical_stop_dist;
+     738             :   }
+     739             : 
+     740           0 :   res.message = "hover initiated";
+     741           0 :   res.success = true;
+     742             : 
+     743           0 :   changeState(STOP_MOTION_STATE);
+     744             : 
+     745           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     746             : }
+     747             : 
+     748             : //}
+     749             : 
+     750             : /* //{ startTrajectoryTracking() */
+     751             : 
+     752           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     753           0 :   return std_srvs::TriggerResponse::Ptr();
+     754             : }
+     755             : 
+     756             : //}
+     757             : 
+     758             : /* //{ stopTrajectoryTracking() */
+     759             : 
+     760           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     761           0 :   return std_srvs::TriggerResponse::Ptr();
+     762             : }
+     763             : 
+     764             : //}
+     765             : 
+     766             : /* //{ resumeTrajectoryTracking() */
+     767             : 
+     768           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     769           0 :   return std_srvs::TriggerResponse::Ptr();
+     770             : }
+     771             : 
+     772             : //}
+     773             : 
+     774             : /* //{ gotoTrajectoryStart() */
+     775             : 
+     776           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     777           0 :   return std_srvs::TriggerResponse::Ptr();
+     778             : }
+     779             : 
+     780             : //}
+     781             : 
+     782             : /* //{ setConstraints() */
+     783             : 
+     784           3 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LineTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     785             : 
+     786           6 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     787             : 
+     788             :   // this is the place to copy the constraints
+     789             :   {
+     790           3 :     std::scoped_lock lock(mutex_constraints_);
+     791             : 
+     792           3 :     _horizontal_speed_        = cmd->constraints.horizontal_speed;
+     793           3 :     _horizontal_acceleration_ = cmd->constraints.horizontal_acceleration;
+     794             : 
+     795           3 :     _vertical_speed_        = cmd->constraints.vertical_ascending_speed;
+     796           3 :     _vertical_acceleration_ = cmd->constraints.vertical_ascending_acceleration;
+     797             : 
+     798           3 :     _heading_rate_ = cmd->constraints.heading_speed;
+     799             :   }
+     800             : 
+     801           3 :   res.success = true;
+     802           3 :   res.message = "constraints updated";
+     803             : 
+     804           6 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     805             : }
+     806             : 
+     807             : //}
+     808             : 
+     809             : /* //{ setReference() */
+     810             : 
+     811           1 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LineTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     812             : 
+     813           2 :   mrs_msgs::ReferenceSrvResponse res;
+     814             : 
+     815           1 :   auto state_heading = mrs_lib::get_mutexed(mutex_state_, state_heading_);
+     816             : 
+     817             :   {
+     818           1 :     std::scoped_lock lock(mutex_goal_);
+     819             : 
+     820           1 :     goal_x_       = cmd->reference.position.x;
+     821           1 :     goal_y_       = cmd->reference.position.y;
+     822           1 :     goal_z_       = cmd->reference.position.z;
+     823           1 :     goal_heading_ = radians::unwrap(cmd->reference.heading, state_heading);
+     824             : 
+     825           1 :     ROS_INFO("[LineTracker]: received new setpoint %.2f, %.2f, %.2f, %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     826             : 
+     827           1 :     have_goal_ = true;
+     828             :   }
+     829             : 
+     830           1 :   changeState(STOP_MOTION_STATE);
+     831             : 
+     832           1 :   res.success = true;
+     833           1 :   res.message = "reference set";
+     834             : 
+     835           2 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+     836             : }
+     837             : 
+     838             : //}
+     839             : 
+     840             : /* //{ setVelocityReference() */
+     841             : 
+     842           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LineTracker::setVelocityReference([
+     843             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     844           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     845             : }
+     846             : 
+     847             : //}
+     848             : 
+     849             : /* //{ setTrajectoryReference() */
+     850             : 
+     851           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LineTracker::setTrajectoryReference([
+     852             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     853           0 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     854             : }
+     855             : 
+     856             : //}
+     857             : 
+     858             : // | ----------------- state machine routines ----------------- |
+     859             : 
+     860             : /* //{ changeStateHorizontal() */
+     861             : 
+     862           6 : void LineTracker::changeStateHorizontal(States_t new_state) {
+     863             : 
+     864           6 :   previous_state_horizontal_ = current_state_horizontal_;
+     865           6 :   current_state_horizontal_  = new_state;
+     866             : 
+     867             :   // just for ROS_INFO
+     868           6 :   ROS_DEBUG("[LineTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     869           6 : }
+     870             : 
+     871             : //}
+     872             : 
+     873             : /* //{ changeStateVertical() */
+     874             : 
+     875           6 : void LineTracker::changeStateVertical(States_t new_state) {
+     876             : 
+     877           6 :   previous_state_vertical_ = current_state_vertical_;
+     878           6 :   current_state_vertical_  = new_state;
+     879             : 
+     880             :   // just for ROS_INFO
+     881           6 :   ROS_DEBUG("[LineTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     882           6 : }
+     883             : 
+     884             : //}
+     885             : 
+     886             : /* //{ changeState() */
+     887             : 
+     888           4 : void LineTracker::changeState(States_t new_state) {
+     889             : 
+     890           4 :   changeStateVertical(new_state);
+     891           4 :   changeStateHorizontal(new_state);
+     892           4 : }
+     893             : 
+     894             : //}
+     895             : 
+     896             : // | --------------------- motion routines -------------------- |
+     897             : 
+     898             : /* //{ stopHorizontalMotion() */
+     899             : 
+     900          19 : void LineTracker::stopHorizontalMotion(void) {
+     901             : 
+     902             :   {
+     903          38 :     std::scoped_lock lock(mutex_state_);
+     904             : 
+     905          19 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     906             : 
+     907          19 :     if (current_horizontal_speed_ < 0) {
+     908          19 :       current_horizontal_speed_        = 0;
+     909          19 :       current_horizontal_acceleration_ = 0;
+     910             :     } else {
+     911           0 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     912             :     }
+     913             :   }
+     914          19 : }
+     915             : 
+     916             : //}
+     917             : 
+     918             : /* //{ stopVerticalMotion() */
+     919             : 
+     920          19 : void LineTracker::stopVerticalMotion(void) {
+     921             : 
+     922             :   {
+     923          38 :     std::scoped_lock lock(mutex_state_);
+     924             : 
+     925          19 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     926             : 
+     927          19 :     if (current_vertical_speed_ < 0) {
+     928           1 :       current_vertical_speed_        = 0;
+     929           1 :       current_vertical_acceleration_ = 0;
+     930             :     } else {
+     931          18 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     932             :     }
+     933             :   }
+     934          19 : }
+     935             : 
+     936             : //}
+     937             : 
+     938             : /* //{ accelerateHorizontal() */
+     939             : 
+     940        1004 : void LineTracker::accelerateHorizontal(void) {
+     941             : 
+     942             :   // copy member variables
+     943        1004 :   auto [goal_x, goal_y]                             = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_);
+     944        1004 :   auto [state_x, state_y, current_horizontal_speed] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, current_horizontal_speed_);
+     945             : 
+     946             :   {
+     947        1004 :     std::scoped_lock lock(mutex_state_);
+     948             : 
+     949        1004 :     current_heading_ = atan2(goal_y - state_y, goal_x - state_x);
+     950             :   }
+     951             : 
+     952        1004 :   auto current_heading = mrs_lib::get_mutexed(mutex_state_, current_heading_);
+     953             : 
+     954             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     955             : 
+     956        1004 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     957        1004 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     958        1004 :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     959        1004 :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     960             : 
+     961             :   {
+     962        2008 :     std::scoped_lock lock(mutex_state_);
+     963             : 
+     964        1004 :     current_horizontal_speed_ += _horizontal_acceleration_ * _tracker_dt_;
+     965             : 
+     966        1004 :     if (current_horizontal_speed_ >= _horizontal_speed_) {
+     967         905 :       current_horizontal_speed_        = _horizontal_speed_;
+     968         905 :       current_horizontal_acceleration_ = 0;
+     969             :     } else {
+     970          99 :       current_horizontal_acceleration_ = _horizontal_acceleration_;
+     971             :     }
+     972             :   }
+     973             : 
+     974        1004 :   if (sqrt(pow(state_x + stop_dist_x - goal_x, 2) + pow(state_y + stop_dist_y - goal_y, 2)) < (2 * (_horizontal_speed_ * _tracker_dt_))) {
+     975             : 
+     976             :     {
+     977           1 :       std::scoped_lock lock(mutex_state_);
+     978             : 
+     979           1 :       current_horizontal_acceleration_ = 0;
+     980             :     }
+     981             : 
+     982           1 :     changeStateHorizontal(DECELERATING_STATE);
+     983             :   }
+     984        1004 : }
+     985             : 
+     986             : //}
+     987             : 
+     988             : /* //{ accelerateVertical() */
+     989             : 
+     990         200 : void LineTracker::accelerateVertical(void) {
+     991             : 
+     992         200 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     993         200 :   auto [state_z, current_vertical_speed] = mrs_lib::get_mutexed(mutex_state_, state_z_, current_vertical_speed_);
+     994             : 
+     995             :   // set the right heading
+     996         200 :   double tar_z = goal_z - state_z;
+     997             : 
+     998             :   // set the right vertical direction
+     999             :   {
+    1000         200 :     std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002         200 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+    1003             :   }
+    1004             : 
+    1005         200 :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+    1006             : 
+    1007             :   // calculate the time to stop and the distance it will take to stop [vertical]
+    1008         200 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+    1009         200 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+    1010         200 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+    1011             : 
+    1012             :   {
+    1013         400 :     std::scoped_lock lock(mutex_state_);
+    1014             : 
+    1015         200 :     current_vertical_speed_ += _vertical_acceleration_ * _tracker_dt_;
+    1016             : 
+    1017         200 :     if (current_vertical_speed_ >= _vertical_speed_) {
+    1018         101 :       current_vertical_speed_        = _vertical_speed_;
+    1019         101 :       current_vertical_acceleration_ = 0;
+    1020             :     } else {
+    1021          99 :       current_vertical_acceleration_ = _vertical_acceleration_;
+    1022             :     }
+    1023             :   }
+    1024             : 
+    1025         200 :   if (fabs(state_z + stop_dist_z - goal_z) < (2 * (_vertical_speed_ * _tracker_dt_))) {
+    1026             : 
+    1027             :     {
+    1028           1 :       std::scoped_lock lock(mutex_state_);
+    1029             : 
+    1030           1 :       current_vertical_acceleration_ = 0;
+    1031             :     }
+    1032             : 
+    1033           1 :     changeStateVertical(DECELERATING_STATE);
+    1034             :   }
+    1035         200 : }
+    1036             : 
+    1037             : //}
+    1038             : 
+    1039             : /* //{ decelerateHorizontal() */
+    1040             : 
+    1041         100 : void LineTracker::decelerateHorizontal(void) {
+    1042             : 
+    1043             :   {
+    1044         200 :     std::scoped_lock lock(mutex_state_);
+    1045             : 
+    1046         100 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+    1047             : 
+    1048         100 :     if (current_horizontal_speed_ < 0) {
+    1049           1 :       current_horizontal_speed_ = 0;
+    1050             :     } else {
+    1051          99 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+    1052             :     }
+    1053             :   }
+    1054             : 
+    1055         100 :   auto current_horizontal_speed = mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_);
+    1056             : 
+    1057         100 :   if (current_horizontal_speed == 0) {
+    1058             : 
+    1059             :     {
+    1060           1 :       std::scoped_lock lock(mutex_state_);
+    1061             : 
+    1062           1 :       current_horizontal_acceleration_ = 0;
+    1063             :     }
+    1064             : 
+    1065           1 :     changeStateHorizontal(STOPPING_STATE);
+    1066             :   }
+    1067         100 : }
+    1068             : 
+    1069             : //}
+    1070             : 
+    1071             : /* //{ decelerateVertical() */
+    1072             : 
+    1073         100 : void LineTracker::decelerateVertical(void) {
+    1074             : 
+    1075             :   {
+    1076         200 :     std::scoped_lock lock(mutex_state_);
+    1077             : 
+    1078         100 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+    1079             : 
+    1080         100 :     if (current_vertical_speed_ < 0) {
+    1081           1 :       current_vertical_speed_ = 0;
+    1082             :     } else {
+    1083          99 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+    1084             :     }
+    1085             :   }
+    1086             : 
+    1087         100 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1088             : 
+    1089         100 :   if (current_vertical_speed == 0) {
+    1090           1 :     current_vertical_acceleration_ = 0;
+    1091           1 :     changeStateVertical(STOPPING_STATE);
+    1092             :   }
+    1093         100 : }
+    1094             : 
+    1095             : //}
+    1096             : 
+    1097             : /* //{ stopHorizontal() */
+    1098             : 
+    1099          46 : void LineTracker::stopHorizontal(void) {
+    1100             : 
+    1101             :   {
+    1102          46 :     std::scoped_lock lock(mutex_state_);
+    1103             : 
+    1104          46 :     state_x_                         = 0.95 * state_x_ + 0.05 * goal_x_;
+    1105          46 :     state_y_                         = 0.95 * state_y_ + 0.05 * goal_y_;
+    1106          46 :     current_horizontal_acceleration_ = 0;
+    1107             :   }
+    1108          46 : }
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ stopVertical() */
+    1113             : 
+    1114         850 : void LineTracker::stopVertical(void) {
+    1115             : 
+    1116             :   {
+    1117         850 :     std::scoped_lock lock(mutex_state_);
+    1118             : 
+    1119         850 :     state_z_                       = 0.95 * state_z_ + 0.05 * goal_z_;
+    1120         850 :     current_vertical_acceleration_ = 0;
+    1121             :   }
+    1122         850 : }
+    1123             : 
+    1124             : //}
+    1125             : 
+    1126             : // | ------------------------- timers ------------------------- |
+    1127             : 
+    1128             : /* //{ mainTimer() */
+    1129             : 
+    1130        2212 : void LineTracker::mainTimer(const ros::TimerEvent &event) {
+    1131             : 
+    1132        2212 :   if (!is_active_) {
+    1133         410 :     return;
+    1134             :   }
+    1135             : 
+    1136        5406 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _tracker_loop_rate_, 0.01, event);
+    1137        5406 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("LineTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1138             : 
+    1139        1802 :   auto [goal_x, goal_y, goal_z]    = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1140        1802 :   auto [state_x, state_y, state_z] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, state_z_);
+    1141             : 
+    1142        1802 :   switch (current_state_horizontal_) {
+    1143             : 
+    1144         633 :     case IDLE_STATE:
+    1145             : 
+    1146         633 :       break;
+    1147             : 
+    1148          19 :     case STOP_MOTION_STATE:
+    1149             : 
+    1150          19 :       stopHorizontalMotion();
+    1151             : 
+    1152          19 :       break;
+    1153             : 
+    1154        1004 :     case ACCELERATING_STATE:
+    1155             : 
+    1156        1004 :       accelerateHorizontal();
+    1157             : 
+    1158        1004 :       break;
+    1159             : 
+    1160         100 :     case DECELERATING_STATE:
+    1161             : 
+    1162         100 :       decelerateHorizontal();
+    1163             : 
+    1164         100 :       break;
+    1165             : 
+    1166          46 :     case STOPPING_STATE:
+    1167             : 
+    1168          46 :       stopHorizontal();
+    1169             : 
+    1170          46 :       break;
+    1171             :   }
+    1172             : 
+    1173        1802 :   switch (current_state_vertical_) {
+    1174             : 
+    1175         633 :     case IDLE_STATE:
+    1176             : 
+    1177         633 :       break;
+    1178             : 
+    1179          19 :     case STOP_MOTION_STATE:
+    1180             : 
+    1181          19 :       stopVerticalMotion();
+    1182             : 
+    1183          19 :       break;
+    1184             : 
+    1185         200 :     case ACCELERATING_STATE:
+    1186             : 
+    1187         200 :       accelerateVertical();
+    1188             : 
+    1189         200 :       break;
+    1190             : 
+    1191         100 :     case DECELERATING_STATE:
+    1192             : 
+    1193         100 :       decelerateVertical();
+    1194             : 
+    1195         100 :       break;
+    1196             : 
+    1197         850 :     case STOPPING_STATE:
+    1198             : 
+    1199         850 :       stopVertical();
+    1200             : 
+    1201         850 :       break;
+    1202             :   }
+    1203             : 
+    1204        1802 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1205          19 :     if (current_vertical_speed_ == 0 && current_horizontal_speed_ == 0) {
+    1206           1 :       if (have_goal_) {
+    1207           1 :         changeState(ACCELERATING_STATE);
+    1208             :       } else {
+    1209           0 :         changeState(STOPPING_STATE);
+    1210             :       }
+    1211             :     }
+    1212             :   }
+    1213             : 
+    1214        1802 :   if (current_state_horizontal_ == STOPPING_STATE && current_state_vertical_ == STOPPING_STATE) {
+    1215          47 :     if (fabs(state_x - goal_x) < 1e-3 && fabs(state_y - goal_y) < 1e-3 && fabs(state_z - goal_z) < 1e-3) {
+    1216             : 
+    1217             :       {
+    1218           1 :         std::scoped_lock lock(mutex_state_);
+    1219             : 
+    1220           1 :         state_x_ = goal_x;
+    1221           1 :         state_y_ = goal_y;
+    1222           1 :         state_z_ = goal_z;
+    1223             :       }
+    1224             : 
+    1225           1 :       changeState(IDLE_STATE);
+    1226             : 
+    1227           1 :       have_goal_ = false;
+    1228             :     }
+    1229             :   }
+    1230             : 
+    1231             :   {
+    1232        1802 :     std::scoped_lock lock(mutex_state_);
+    1233             : 
+    1234        1802 :     state_x_ += cos(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1235        1802 :     state_y_ += sin(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1236        1802 :     state_z_ += current_vertical_direction_ * current_vertical_speed_ * _tracker_dt_;
+    1237             :   }
+    1238             : 
+    1239             :   // --------------------------------------------------------------
+    1240             :   // |                        heading tracking                        |
+    1241             :   // --------------------------------------------------------------
+    1242             : 
+    1243             :   {
+    1244        3604 :     std::scoped_lock lock(mutex_state_);
+    1245             : 
+    1246             :     // compute the desired heading rate
+    1247             :     double current_heading_rate;
+    1248        1802 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1249           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1250             :     else
+    1251        1802 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1252             : 
+    1253        1802 :     if (current_heading_rate > _heading_rate_) {
+    1254          50 :       current_heading_rate = _heading_rate_;
+    1255        1752 :     } else if (current_heading_rate < -_heading_rate_) {
+    1256           0 :       current_heading_rate = -_heading_rate_;
+    1257             :     }
+    1258             : 
+    1259             :     // flap the resulted state_heading_ aroud PI
+    1260        1802 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1261             : 
+    1262        1802 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1263        1363 :       state_heading_ = goal_heading_;
+    1264             :     }
+    1265             :   }
+    1266             : }
+    1267             : 
+    1268             : //}
+    1269             : 
+    1270             : }  // namespace line_tracker
+    1271             : 
+    1272             : }  // namespace mrs_uav_trackers
+    1273             : 
+    1274             : #include <pluginlib/class_list_macros.h>
+    1275           1 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::line_tracker::LineTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..b00a836f88 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html @@ -0,0 +1,339 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()57
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()101
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>)57
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&)182
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
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&)3
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&)64638
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)88
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()26
+
+
+ + + +
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..4a1cd1ad93 --- /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-01-07 21:47:14Functions: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          57 : 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          57 :   this->common_handlers_  = common_handlers;
+      81          57 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83          57 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85          57 :   nh_ = nh;
+      86             : 
+      87          57 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95         114 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97          57 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99          57 :   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          57 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107          57 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109         114 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111          57 :   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          57 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122          57 :   is_initialized_ = true;
+     123             : 
+     124          57 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126          57 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133          88 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135          88 :   std::stringstream ss;
+     136             : 
+     137          88 :   is_active_ = true;
+     138             : 
+     139          88 :   ss << "activated";
+     140          88 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142         176 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149         101 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151         101 :   is_active_ = false;
+     152             : 
+     153         101 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154         101 : }
+     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       64638 : 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       64638 :   if (!is_active_) {
+     174       64438 :     return {};
+     175             :   }
+     176             : 
+     177         600 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179         600 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181         400 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183         200 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184         200 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186         200 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187         200 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188         200 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190         200 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191         200 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192         200 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195         200 :     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         200 :   tracker_cmd.use_position_vertical   = true;
+     203         200 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205         200 :   tracker_cmd.use_velocity_vertical   = true;
+     206         200 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208         200 :   tracker_cmd.use_heading = true;
+     209             : 
+     210         200 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212         200 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219          26 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221          26 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223          26 :   tracker_status.active            = is_active_;
+     224          26 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226          26 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233         169 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235         338 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237         169 :   res.message = "callbacks are always disabled";
+     238         169 :   res.success = true;
+     239             : 
+     240         338 :   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         182 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300         364 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302         182 :   res.success = true;
+     303         182 :   res.message = "constraints updated";
+     304             : 
+     305         364 :   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           3 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MidairActivationTracker::setTrajectoryReference([
+     331             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     332           3 :   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          57 : 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..dafc1ffaf5 --- /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:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
<unnamed>71.1 %1190 / 167363.3 %31 / 49
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html b/mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html new file mode 100644 index 0000000000..9f7152d505 --- /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:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
<unnamed>71.1 %1190 / 167363.3 %31 / 49
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-detail.html b/mrs_uav_trackers/src/mpc_tracker/index-detail.html new file mode 100644 index 0000000000..5be41e7f50 --- /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:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
<unnamed>71.1 %1190 / 167363.3 %31 / 49
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html b/mrs_uav_trackers/src/mpc_tracker/index-sort-f.html new file mode 100644 index 0000000000..dfae5dbcd8 --- /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:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html b/mrs_uav_trackers/src/mpc_tracker/index-sort-l.html new file mode 100644 index 0000000000..6a11e9fa45 --- /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:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/index.html b/mrs_uav_trackers/src/mpc_tracker/index.html new file mode 100644 index 0000000000..3dc2c0646e --- /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:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
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 +
71.1%71.1%
+ 		71.1 %1190 / 167363.3 %31 / 49
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..25be341186 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html @@ -0,0 +1,276 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackWiggle(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::debugPrintState(double)0
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&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)0
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)0
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)0
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)3
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)4
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()26
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)53
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)57
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)121
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)121
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)182
mrs_uav_trackers::mpc_tracker::MpcTracker::timerTrajectoryTracking(ros::TimerEvent const&)406
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)468
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)496
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)497
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)625
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)678
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)723
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)799
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1269
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)2224
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()4992
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)11236
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()12964
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)40674
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)47666
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()47759
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)47759
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)47759
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()47759
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)47759
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)64638
+
+
+ + + +
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..3eaaf7f20a --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html @@ -0,0 +1,276 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/mpc_tracker - mpc_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1190167371.1 %
Date:2024-01-07 21:47:14Functions:314963.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fd0916ac26fba425f0172b1cca4e720aed3bec07 GIT binary patch literal 12605 zcmV-DF~ZJ?P)a4jOLpDYagLGgo1j_@NTVeIDWp9ZX?jSE9Oa^x zV(daiV&r8a1r%eHcdg^27^4(WIz}lmCBkJ5V(iC;F=`pk1Gjc92JGti8)Mg}NsNmb z`<^h@Zd$6cIJ{@jbp`i? zt`{rgW&~p}I#RC5UX$rwF`#r3qk0Som!g4_{fPqSi z7o-4ibz|(EnJ_qqR)bL5e;l%6yb_Fs=7v`XT-AKUv#~6FgaLGeb_hG6OC4j8Gw?`6 zv(8~@G6MMtK^SFYZ0fq^Q3-z*oepJP`&ls?Y!&eCcH-WCFO1NByEfo8;UC`LB^do0 zFdtnEKGHPS0~+86xGV^|1rT(-hUr%mo$}E!5IU(E_?Wa6u!;gCKG!ZDWY?&hV%HR` zu4`bm)1pp{u5(SKGgkI14m@y?7z=(J*%A!M(t2iuoA9!KKWenScdYu(-Ns*vYtRskfnDDgQu?n1Id6~?$)i=~3%9#q;|zWt1=qjgLKMojiX@Iix7>dt^@i#r_; zm|FleAklT`rF4Oto^@Kr*x}J!!hiALy7<#8Kuk7%3dw-3R?K8snOG-MGmDhDWP!wDhnZ z_Dp&pr}gAa3gwWfw>$KQmjQ1H)qNBjj0x50>hGXBd{d04fS{ufuna zehfHrBXgAkN-^fzh6e0qkH4x-W~qTaOW%;n7)y77Nb!_FwOudX0wlkq$4CGZc6f58 z5C5vhh=gKIkT*;P0>|nqdU;VtnfhH?x-Onm6sQ75p(jdg$U^mTzmwq%+A+@2;bFpx zfanw5)-ZCNE=W=J+|Se`p?vUJge~aGlTKOsn=knhqhzD$KwmM=k>qm*X#Q~ z+aA7I!0*4m9_4m>)F3>ddJ6Ax(@Z=ST~ksRA4N(y(|_>f(p#_(M;GZUt8c-K7O)dGVRfO2BY zhpJYE0#M#{KYM-=ACnKVY*GM9yS9-9{&uu&SD3jJj`54@c((#D)Av1h*E|;I+5m1+ zz|%^`0HKIfAH_wR0*W!7JnS!30BHO8hw$+aa15Y)uDP@A z)#5SoBMy+5J(je?{{J%2_v`xx)tsN+P@4y6V9R4HdDUXubyQ#9>_|4S*DZ_@17cuZ z*BLg3YB|Qh?xF>t!sxMO^BC!#33x(ZWd$&oH3+Dt`pjpjb1%#UEp4D{7Vyw@9f~Vj z_Y&_|N|)A-Xgz~Or3|-UcB^%c#D9pD6ru>SM@x*49%t#v^~6r;A?Lh0@~r`DBE?*9 zku%>Uk2vnZDD9GGFjbFe3c)~B#6;HYDF=^6jFb-N0VVDS!41h~_$X}G#xB{i=Xi#b zOEASJnU>1L&hcBj3cbHN#zk~o4sMw&(CUv`*Al=(jIjVHWe*|WN4|qGHZG;5^w(F_ zNdaA(#gp~wn1IKa4=(^Vv4`Sy08~=#zwn{D8N-=PwqvgMlUZDW)mP&kFThlAfXN5B zuHn3}fPSVKb(wcv1#}oW{hJzb#2V1sW$ycjR538lkp5D=H9EPElG34=iQ+@OTQPG7=k}u<@F?p$5BRWh_I#y>EJ|?2fYL)nFCVio+qbgkr>On_ z>7lfl81VluJ#V9$(3EkEz-2=XQw{j~ssmnjlz{3mGy5*Oj28nQXB}|#MUJ<`B~vJ& zWzzyyiEh1%qt{iy7y!@$O98D=V3%KRTNyE{8(hx3_E@r|2-*cGT837_1@<1>H3cZ^ z+7zOaq!|i(eyi>&2nD_y7HZrQ#v=tBy{^-qi(?c6%36BI?!q4~*MIo&)MAuMnWka$ z`W^x%+U}HvrrCo5B$t8*tSkiupsf8riAfEBT+J|mIpi&x6lGP7neV zcRiMvFLx;wTv=$06^-c=*wpr>J5;rmqfV^WDHsTJ6 z1C(Rjj}j)f>wTxQHMHqPOUNgNW#CX9q{Ww0UG6?3$%PxDmw^^g$wf3XyNxP5kL#Ng z&ri$D)ByDut>BEn_~w+nw`8$xRSxPgiZ3A2HNyw6Owm1oi6u?(N%ve6i*2yGf|Na5 zb(Y&Og5R*)2i_DUfC1xxF`gK22gWX&f?8^gXRh<&4wFKk#SpN7Q*DZIH{#X#YsFah z#EEr}xa*0!=RT;A)$QvGIZ+vZsp1F_F&6z-MD2$wuk3f_oCRVkZsZ~{uMF^A#JEf0 z%x@l|Ed3azUBB;+Ju94}cPkbqAydP}ZQ50<*);Or4ig z0M*0T^<$VLSpr&-P8 z0#k2i-jd5eSpYO}iO9PK2}|+BhX=EzJ^+98A%p4l6@h9|n))oZ>XaJ~QgY!+rZ%e` zzV=xd8N>1XB3HG}Wp`y%)d9sAt%uDN+8a~_VGA?uCfg#%*xN!myqiCp#63m~@f%D~ zQp|0W`)Y2@0yT56)!npL^p-7CuWA2@zwXxf-EVp%i-I-gh#@cOv~D5Uz2 z7&S4JPkJ+wIsvUS!FaBIozKIMPvJq_^|}JiVw*=UO_2=(v}=gyne?CjtpliHP4=1d81 zO6RDT+t*@*z9wc?^celLVSMdlq#Mv4M-j03=^Gwn!m}R+grcPLa)I}6{VTY4@0Qfp z$HXoY@O_n!kcJAwiT5u$r;w2LDS$m=%qoMZp64lwSzL;f^E8VXKPpdin3;=$Jl)jh zzi8NyM%N>S;bm;WBq2t4Hw*=k0CYS34r7$;VmRWC^v;7g9HTuyB%BKS>Fnq*{xJkE z6pn4Bt|Q>cEMMwWgn#!b3Lw^x9uoa6QUGVWMaoYl&gd=xD!MkBfj(HDDPUY3B=!2P zYcd-!MhV>Af!`Fya6zw!{eziqsTC9gOYLJCtIx4mojJAoQ~~$eR(N)c{Kj<++k$?M z`5X7FVOUifr94|YnX<#r8_96-6avZT?{i!2p51ODwhBtr+2%;P<+4}ASd%o>7~vte zh%ucTLrCs0aDbw&-x;JGOR9NaDR_0MtvJ79MyW#GrUKkTo*hg*eGj6HWBklqHrS(0;B z1{7j^TvdUu_5XZP&z6OP)2e5;9`VzB3xHJlj?n*0`Gy!ycf*AxuaQR>E6p!#4_JCR*?gp>>l3Hao=|Z#1F}hXnK8UOp9=vv zr5L)b{)kX|s42u;@-t(JtW2T@JhIvLiMNQ48o;t&@sk*yQ=MJfX#n@b-A8(P+}*|o z0=MO3R5ZYzGJ~}2DIbtFHDINB_;l%61Di;y<67Gbp3)SMyfpWKa~!@Cyj(Izrr3F< zASQ6~bPk%Fz_uIl-(0v9bj&6={ktCaWn>W9RgRE&nIrqnQ)AYWZnsA$ zPWDyw7PJs!u~q;tqr6s^3s~=!YK@993J&aFpHw<1y6d}pkV*J;loE@XVrIgdl3CHj zR*YJCtT~3nfSNJ>U*&PYrKVgS6%1#^43$DvOyQrH@j@|O?e)vq&x#q8vp+E_W0VWU zWHB3IM*gUnbxh%(nDIg}Q_Kd{fTY8I@{?TCX}DbRg1rz!(mwkJl03NXN~&+6=o`q| zCxc+@9J+WNs5UU*fEODYgWdgw;p)ek)`E=*bDPlh(dgVyozcz2jE=qBuph@X(_1oN z2G9UY0gpPM;|m6STaZl@$8zbArkmX2yJW3*!zY^%*^}91ux4Xb#JVN|K;APojInfu z6Qt*ZTz2EOLT)ca9BWJb8vA+?JG?lB%@_%pg@9VYE$Dy|f{SjOKTB{&32(uOOStyK zMqKJ5`CZRsI@%D`V)if@odlpBW7zWPtjZ}h*b=7jX)$_Y!1fxy>BLB~ng#66E=a=_ z3;7C@FYSN`XI(>4EE1E-?3m<>f=vHh~(PC~0-L^O7f@y*LMi2oBM zo2ZG&Vj z1HL3f)T|j=r$D7*a@vTUOT-?Bgno=s)*MEWB7Iw}&#t#qsrgpEoqA_)!Kki@=A_Pc zEmh4ELfxmwrSu04uv3@i7fPq_4Th#0Y{MAyfn{AQF-DTUC|gr!?RopI^d?bak*;>} zEdKbFd@y~Yo3~1p7O7yhX6#7^5m_XUrx1_vCstwask9i;0A#9J9-oXY+$iBM@Fl!Q z?V1*&+o}B)O$dM@np@NL|LJ{KFNA|GHER(z{;yM9BvOp>TzepA{(Tk}P#_h*?Ya7) zXRZ;Ng4?dMv+JZ|vo7zWnEUbXs+7TA*P@dfw1!&%x;8BU)L}H9{c(36z?08Zow@+U z&V6y$a|_B*ZH<8G=k6#>5daD?D#qwhF7W=rUqbXMW9*BM@ju`y8cD8&{eQJ-w0{nluF5V0Y#sE@MhSYb;!zHD-AFLDxV$ig_{1?x4 zW!E>$>>|e>DcRqwev%tr1PsH3OCWx)D}+{9V6PYQu#+N7T&D`|xx_8+8NM(b9EjSP zky`=Y&A!}YiRAhi!98Sfn>x#05?Nq|C})zOnGtgCJ)nPbJ~H}en(hJpT@pxP#0=+N zwx7fB!EhHLB0h@k#|;@m{KhV}05~5A;qGAJe-*K@YfYF z+rz;?-nDT3tK-AXc#2OkfG5`2Hx4Z^ny$%b+aO9MptS2-6siix-v}se!a}i1MS2}I zC2tobQ?fIei0y#pTURL)zs4#BJZd$@*)g)OHnstRF}ik`%C#E6(N!;rhq6g_X*bRA zloxo6$^VK3jO4=geP{vYW6abUM>U4yFp8$;on331JDsydP`&hI#IRIv0ysC<;nB{6 zrJKOmpEmdj#LQ3tI%V$$>gyLDXuujkt!hp%o`J9a7aG8~zi8ua(%@QeHgY$h98dfW zU@%j(gsvYhW6uWEa%~D_7=@1EEHd@^3O)@rw&l~a!iSF=#liT zN2!G4+(EM?!FabB*nItPImUM$_oNv4^GrL&V4KyPsd?Y|ano4M66=G;c;t^@O|iqL z6++`qn)aailQTnA@$Z}&Lib2#X2Lr@gU{Eu|JUK{mz-zz^r`400c<*^WXmex{yo

A5F=@zXmDZP2@~K=FSK`yN+FH`Uf7{1mC=K;do1I6nFzX_=5;eim!6 zYsMJKWXl37tdujUu18_qAx5&eIN()tga-eO+}h(d*Pd5xJ&p}3MamsE5AZ}99v7*d zU<@A{j)zMS>zSD!`(;9qw2J89BECuEaFku@OH~AQ+l;({AJFwxo?gvbPD}V<8l+H|gufX9O z1qgwwfH1yCNemW-@qh@p4466r|5ajAEOd5USo5vRH3qDEEHK8rw4pJ zf}ql4mJ`Ed*=1bSjBzv{Lr(5$!^A=wge@qy`g+G{WhG*nf^{tgobYH{Vj@_Bq3iNf zqwKNBkPJ`&jxRjrO?q$HUo%hh(eCUQ3o@%K-Ce(bj<@kVAH@v!HihZAV3SV-tk9x0 zXAU5Xgt^s#vFyZA*DMgn_r%pjTnvRz;4SWzFa4KaGs3FC8+u^{MzZ7N3mMgO!)%wB z%NQbUn{vtGZL_b70YZ0M0(XsN=A$v`sea?zsr*vFfRmO&(I=!N=4R8k zLv4*EXuV~HS};V<)(83KnaKhU@))&gAu;y;C$`*zMdtPE#T9E&0M2+kc@O+^|K^*H zCXgHypI#|fy}q#)Mf!WyU7XyK67@Yn%>HbOF?{?yn_>u{k{Dtxih%qFIGcidJUIpO z^K<^$6dz(!{3)pddvr6CPS2l`!WhYQQUK&X@Kz<&8xn&tzf<>2Rw*F``pqgOC1{LF zfn5KI_lDUq3dUChO!!j<1k;cSMS3Ry0sUt)UnVBzH!d#M^ka16VPXXv#;CG;V!%L= zYAG=r+~Ca9+q8aslp=Rc1pr(4(gQxgyJGZd*kdduyE{PPrCmyl`G?d8&BWBR_h%9F zH#2`T^EWd!V&vPfFk>mDx)#duB>;VA`HrshHOZbL^HRVwUWb?OhJDZemX-sY7;u^# z_&}(!?>c&C4`=;^(ZZkd=!aN?`Q^ivo=<62;O_+{BV>_gnHaz+Da(;R{A{M`n}h8ZGx2IvU&V;RWr8L9`qy^re{qYy zx2GB>jwL|*d&@}yprGp%5D&V9F&T8dYd7frG5$J0L9^MZ$GHU*1;7h-JmC4ht`&ga zqV(&>=w-hQSZ~1?Ce#IoFz|8{YC~3fQ*X0I@sRH`0#{){Hu-+8g@OZOOdy!Ml zMA%Oapo%@aXUM)=2X=Nghk%ifp(b9;%Fu1Y;Kku3QqN+ckjRVPdtTYpD)AiN0(a~-dIZ>PmZ8Z{}m>mc#PFY<1avwpZott z_4u2L4LPMLni;AIqQ5e#X_|k}T>m>%-~7{;Sp!*6?x1(S;kQB8;2w7h)}H3wk)C=y zK4Og+m!(>n^BZQc58_Wh4v?Nds{b8L@o!Rni+&u{hh%*GH`mO+Np(EF>~X-T9dJpwNOejlJ-oPAatr4uuPUd1nZMu~CBGrof=hsi)k>eI$|_3I~w6UqH-N zRQrz?WmrdbtZOkO9xuvi<$6EloK;@G3~w>k3(7%7A2vj1{yf9CHc;|#ny0YWsJezF zBu%|R*R2V}f0@u?Umg>0*tUl!+@Ef)jwXbP|hvU7S(3#oLtkg=mS}Mit{A zi4nH+VX#<6yF*!`LgOZi^w>>-d@uUurws7Y8352}%;UswejXddc)0(osZJlzC4AH0 zdr=J0s@Z1Zt~XFoE{VNt#uzra#Z-U{dlNA?Aq07KDxZdiW_Z}X;0(CO(&S@&BpTo< zpUJ8eS-T;!OS{*!7ZJ~NqoZD8kxpUQh?l!3XYuPX)glhi{ue&v7?F%|Pe(~^II1a3 z<`8T+;iw&sQlcru7&)nU?=i)#5jQWk%<5`uW4WrUcgt`QzZ-7teX=`SdTT>5{+WU3 zl#I6PC1xzRXo)p0HbZ-sAVQ`jt0FW~j4<)TmsGGZ^$A9=q|JDQfs_La}z59t#M zaHm{ZN8g5|0tCYEm}(h&%$jjV*DEpc>>~6QRqg{|j4lsYHM&%)d3Z1|Gc{B}x^O@% z>4FVUO_-Ts`1O_P=#n+ZR{N4F>(cZ4WPsnH$xP5 zK73cWc;juSAqex;YQ34%UMO3I1V-{SH>FDT_=8VDy7#y(lmT55h;Bm)Z|Ok2vr_J) z=)J132e`S40+a%dc(fjB&UFo5whj-7HHwz_kNaOU!Zhg9o{X?rVWU2$1D=FMqYF4C zvI|Meur;`tBE=4RIv~Xy5BLGTlz4m`6-&0+xjk#i&FTy>(aYpAjKS7RE4nkaX9oic z_Zg2ddw|C6ic;aG>0%Dh>JXM>yzC?6xb8)gCX zS`9hZZIu|&HvGs8$n><}Pd4wrmGW=4ILl-qP9PZ@Px>_3COyY)ooRcxpNWibxaWda z))KEj-BzJ+C~ZQ+N9SQby(jUySaK=LSn8A*ty40$V-6B9L_c?4EJ9)uB4P=_Q+K^@ zFbdYI?w-pZh+~XV9`WOjqzH~$h32bZ!OF!9j4{5?OT4xU#t7iL$2jAD#wD`Tx0GOJ zV%b!GwEhW(gE7X7Gs>9?82$?LLP&*@;p4dNhylyxh>r-NVD~wIqE(n;bV!M2c2^Nb zO?y{`vq-ZYG;O$MHZK1-X=WB8WBkYgkJz;U;~@atQep@n03@X_g{40hsV3TC0YAVe zB{ZZ+sypBNP>eB3nCsbp@9#i&-H{X(E`=tUk8#i*xW^a!eMf~;(=;~j@2YUx>jeCn z3a8Y}%mCDn@vp)u;%3LLkS36%>}kQcljvG89>Cd?O+wl=;}vp30Wna37^4HMkI`e~ z>xVaD90+J=M^wA0eFrFOzfF-IN3KenRm#a3UV4n^1>BzkRE-ff7O(;1G(HkVXeR-x z#wg+AFWNqMVuVy@7)iPOkV8dbMYXZ#jQZ|~U$ZqKPX~S0 zg#d|N!}0O39&h)rZe~0VQep$38Y921eg=>S|B$S6pA|P4Bb|f?8@X+7v*# z*R(QRc!#VHYpez~#=PMexc8);`@>_{$$M_{=1jB@+SfIdg%p1i#_~S%Ny~NIlBGj9 z@|CDwGeOKV8F7DAuj}33wDO^TGXRxigqnh^+0dXV>a}sc;{96jmArpPQ*>hdUucR( zCpFhH>#aY_Hw5?j^ja>0&xU-su+()x_}sO)OeK;eU~OD(ij7uqI<(p8Qd=!O7)!|- z3XHy_#g6hh^LNA>#^|^g3JCRMEIoX0 zqEcRLoQfioBz&q~9Z!leCSDA}7^C(EjZy3ld&Iue6dp8?uH;=ZKL-O|zoUY22#h@7 z=xmo_JS7-71xSviNhpd^p~48uFFgI=i(LosuPhCzAukS~>m+7;9ummX3T0vZQ^O`g z&lT95&7QQb7y^_Y0X`!?B;7vxpHNIV$Za3rTKjMNsAGiN!IMhw{L&?KCJ3Wb?Db0G zPIaYPL_%>vr-DIqtGW79U=am4xZ*C+bKV|I4GlhUS4g#J|4^l|%DXO7(TK6WBmI`V zVZRH7FV?p}?Xkw_I* zB2OQ^#5~HFnlL_JarR0oWy&h_*oF`@RY@ULZ^&+8&+zL4;UVsE6ZWj=dbW}p;b#wv zG?|xlvfKeu$ew@q^pUn7Q+xe>GsK3zqIVhcS3GmET>^|V-C<*WQZMIs@$MF)glEPH zCmR2VPs*98z9@cLC;!iM9ffg0+rmBBC;Fc881#?_72wyI84n^aH?Bm)VXF+z13cNH z6T0;D{KHr4V^3^_qzcqEPO|Uo+Qp@?fDObHQMNN2paxtc0Iy#Xfq5NW1C}2t!^Zf003K)e9pYkNZP=pVmmI0-mEnmb$iPH;aXT5+Un_G*Nu&R+<=8eMi@@e-D-(Cz-(;P8!~{|YfkPygNJ z?|B2D{VI3vi>aO{Jp=9@Z6lUX8p4$6u}4S>X?JBb2h;N zf0pzxZH<7*=duF%08ofgGsdO@lSPcd+qsn9)uqZ(0Q)$cpWDteML_$~`AF#j1=9iz zlr0B#*Svma3Mi+#xJ^gEPfBN_PjBG>9P44?&3piunU7z$m0xa zZ}5xSTrn~%qD}V->6*y^8*K%xTdRl z73X?hBvpp=J+F07|a`Wi5`eSfw6}@%1VVamE;L!~NG=oa|CLdpFmq zP?NH2044gYWsJpg_F#;#@Fdl>W{iVRuiFN#+&xr^$NF>*ps3+(_@KQMsyD>st=EX_ zdAtPTC`4Y(1jG}EjkpP>!W^F_e0iRFfBtF%iqxKsHboX$zyXS1$(!+3B|~(xWlsng zS)_Jmm_%)iQeuh|*M{-f$V&HQ*ZW?5NmNzd2-@X#^?(wb`dDB;MmyKxn~Z5{ci3Kg zA1w7~KBHgr6)v%i5%`?oYVr>;-0%LyLoMI3rjJ%cOEwpZd^xwiub`a5Use{JI>xCm zCJw`K<*ai~r*GQ`0By-?9iV88R}-m&E#(hURo*sxOfl=S95a*!bH3sde}U(wza|`f z5?a!hP2n#p;*Hwj#`pmxx4j}=V$7JmdeevcKXdYqgNgSE%1ge%zWV!(rqBCwM-nMvhV1^}6uYGn1}i`cC15B<$K_ zObyW^BdG$oqvue>fSNJZSUzE{TW5xS$fJSkf`u{@P&LN9<)`6kGgF=$JVsp4H6=zx zbw$KS0v-TRHAb=_z)3Sx1Hxnc*Jp+RoG>$DNX9+b)_AI6I}RbBcbpFg9DM;XKIMIc zdlrcq_)9UULF{!MPV}tTi%R9B1AAmtC)cwDgShGRQ7ZP`dY*AN-nPcMt-kNon*22U z0aHDp&<}m(kvv|tD`zHtnz`58-OR)%5*R=~g^I2nLR|oK0b@nq1}~R*j6>4X+dS+h z!f(C16l`~i)+s;X-30a$4iV$0rT4fOgZwLmpsv};R`Su8oL}5ciVuZCT_JI6sxX~?t}mqyyyvUg__~!KerIQG~J6`bAd697ZAR2 zfGrpg|4as6YBLvV~EghG#cr=_9fIRfRk_V^yT7ZJCC4g>J!9!QZNJo1Rfba?<2NX8@+Ti8_n+ip| zW&xfsdZIh00$eMWF7MhrALNt}fb8jLI-rvsR?LKrj#cPeT-$-o5AqIs3ZR_oY}chk z$P}nSr4JZWVR+eFYK+tjHkvdak zHx(y&O7VYbykaX=y_91_rhWXj6{9<{hZJ8Iu}Lr2GwUxug=A*o0Raodq=29!+R((3 zc9=Kgb1(@Xe;qNWRw#(dDp}JisY;aOrWxZSJQ@#-!zZXzRIRHxq;J_el2#b@!33i>8pl0>&fRkj4YH zj*&{$P*Yt|Du;TEdp%UTIWUcNytH7%?E4u&-59AEX~B3KuFs5?EUA0*aL$Xa?9QuZ zfx4ek+5Jaze3B{rA4W9|t8be7DY$}rO6UGg9b(edz|p}(U(_t@>vUj|y=w|sGZLL7k0ELiL1&6+R{39OEF9AYB@ fY1zlKdY=CQM^fWlMf+_K00000NkvXXu0mjfGj1<- 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..739c790981 --- /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-01-07 21:47:14Functions: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..97e0f3dec0 --- /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-01-07 21:47:14Functions: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..e40a4281be --- /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-01-07 21:47:14Functions: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..c78823423c --- /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-01-07 21:47:14Functions: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..19422f060b --- /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-01-07 21:47:14Functions: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..b0d0806640 --- /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-01-07 21:47:14Functions: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..e6927728fa --- /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-01-07 21:47:14Functions: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&)3
mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()13
(anonymous namespace)::ProxyExec0::ProxyExec0()57
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>)57
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)169
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)182
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)64638
+
+
+ + + +
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..82f6e51264 --- /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-01-07 21:47:14Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  *     http://www.apache.org/licenses/LICENSE-2.0
+      13             : 
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : // Common conversion functions between geometry messages, Eigen types, and yaw.
+      22             : 
+      23             : #ifndef ETH_MAV_MSGS_COMMON_H
+      24             : #define ETH_MAV_MSGS_COMMON_H
+      25             : 
+      26             : #include <geometry_msgs/Point.h>
+      27             : #include <geometry_msgs/Quaternion.h>
+      28             : #include <geometry_msgs/Vector3.h>
+      29             : #include <Eigen/Geometry>
+      30             : #include <boost/algorithm/clamp.hpp>
+      31             : 
+      32             : namespace eth_mav_msgs
+      33             : {
+      34             : 
+      35             : const double kSmallValueCheck         = 1.e-6;
+      36             : const double kNumNanosecondsPerSecond = 1.e9;
+      37             : 
+      38             : /* MagnitudeOfGravity() //{ */
+      39             : 
+      40             : /// Magnitude of Earth's gravitational field at specific height [m] and latitude
+      41             : /// [rad] (from wikipedia).
+      42             : inline double MagnitudeOfGravity(const double height, const double latitude_radians) {
+      43             :   // gravity calculation constants
+      44             :   const double kGravity_0 = 9.780327;
+      45             :   const double kGravity_a = 0.0053024;
+      46             :   const double kGravity_b = 0.0000058;
+      47             :   const double kGravity_c = 3.155 * 1e-7;
+      48             : 
+      49             :   double sin_squared_latitude       = std::sin(latitude_radians) * std::sin(latitude_radians);
+      50             :   double sin_squared_twice_latitude = std::sin(2 * latitude_radians) * std::sin(2 * latitude_radians);
+      51             :   return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude - kGravity_b * sin_squared_twice_latitude) - kGravity_c * height);
+      52             : }
+      53             : 
+      54             : //}
+      55             : 
+      56             : /* vector3FromMsg() //{ */
+      57             : 
+      58             : inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) {
+      59             :   return Eigen::Vector3d(msg.x, msg.y, msg.z);
+      60             : }
+      61             : 
+      62             : //}
+      63             : 
+      64             : /* vector3FromPointMsg() //{ */
+      65             : 
+      66             : inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) {
+      67             :   return Eigen::Vector3d(msg.x, msg.y, msg.z);
+      68             : }
+      69             : 
+      70             : //}
+      71             : 
+      72             : /* quaternionFromMsg() //{ */
+      73             : 
+      74             : inline Eigen::Quaterniond quaternionFromMsg(const geometry_msgs::Quaternion& msg) {
+      75             :   // Make sure this always returns a valid Quaternion, even if the message was
+      76             :   // uninitialized or only approximately set.
+      77             :   Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z);
+      78             :   if (quaternion.norm() < std::numeric_limits<double>::epsilon()) {
+      79             :     quaternion.setIdentity();
+      80             :   } else {
+      81             :     quaternion.normalize();
+      82             :   }
+      83             :   return quaternion;
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* vectorEigenToMsg() //{ */
+      89             : 
+      90             : inline void vectorEigenToMsg(const Eigen::Vector3d& eigen, geometry_msgs::Vector3* msg) {
+      91             :   assert(msg != NULL);
+      92             :   msg->x = eigen.x();
+      93             :   msg->y = eigen.y();
+      94             :   msg->z = eigen.z();
+      95             : }
+      96             : 
+      97             : //}
+      98             : 
+      99             : /* pointEigenToMsg() //{ */
+     100             : 
+     101             : inline void pointEigenToMsg(const Eigen::Vector3d& eigen, geometry_msgs::Point* msg) {
+     102             :   assert(msg != NULL);
+     103             :   msg->x = eigen.x();
+     104             :   msg->y = eigen.y();
+     105             :   msg->z = eigen.z();
+     106             : }
+     107             : 
+     108             : //}
+     109             : 
+     110             : /* quaternionEigenToMsg() //{ */
+     111             : 
+     112             : inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen, geometry_msgs::Quaternion* msg) {
+     113             :   assert(msg != NULL);
+     114             :   msg->x = eigen.x();
+     115             :   msg->y = eigen.y();
+     116             :   msg->z = eigen.z();
+     117             :   msg->w = eigen.w();
+     118             : }
+     119             : 
+     120             : //}
+     121             : 
+     122             : /* yawFromQuaternion() //{ */
+     123             : 
+     124             : /**
+     125             :  * \brief Extracts the yaw part from a quaternion, using RPY / euler (z-y'-z'')
+     126             :  * angles.
+     127             :  * RPY rotates about the fixed axes in the order x-y-z,
+     128             :  * which is the same as euler angles in the order z-y'-x''.
+     129             :  */
+     130         415 : inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
+     131         415 :   return std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
+     132             : }
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* quaternionFromYaw() //{ */
+     137             : 
+     138        2067 : inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
+     139        2067 :   return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
+     140             : }
+     141             : 
+     142             : //}
+     143             : 
+     144             : /* setQuaternionMsgFromYaw() //{ */
+     145             : 
+     146             : inline void setQuaternionMsgFromYaw(double yaw, geometry_msgs::Quaternion* msg) {
+     147             :   assert(msg != NULL);
+     148             :   Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw);
+     149             :   msg->x                   = q_yaw.x();
+     150             :   msg->y                   = q_yaw.y();
+     151             :   msg->z                   = q_yaw.z();
+     152             :   msg->w                   = q_yaw.w();
+     153             : }
+     154             : 
+     155             : //}
+     156             : 
+     157             : /* setAngularVelocityMsgFromYawRate() //{ */
+     158             : 
+     159             : inline void setAngularVelocityMsgFromYawRate(double yaw_rate, geometry_msgs::Vector3* msg) {
+     160             :   assert(msg != NULL);
+     161             :   msg->x = 0.0;
+     162             :   msg->y = 0.0;
+     163             :   msg->z = yaw_rate;
+     164             : }
+     165             : 
+     166             : //}
+     167             : 
+     168             : /* getEulerAnglesFromQuaternion() //{ */
+     169             : 
+     170             : inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion<double>& q, Eigen::Vector3d* euler_angles) {
+     171             :   {
+     172             :     assert(euler_angles != NULL);
+     173             : 
+     174             :     *euler_angles << std::atan2(2.0 * (q.w() * q.x() + q.y() * q.z()), 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
+     175             :         std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())), std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
+     176             :   }
+     177             : }
+     178             : 
+     179             : //}
+     180             : 
+     181             : /* skewMatrixFromVector() //{ */
+     182             : 
+     183             : inline void skewMatrixFromVector(const Eigen::Vector3d& vec, Eigen::Matrix3d* vec_skew) {
+     184             :   assert(vec_skew);
+     185             :   *vec_skew << 0.0f, -vec(2), vec(1), vec(2), 0.0f, -vec(0), -vec(1), vec(0), 0.0f;
+     186             : }
+     187             : 
+     188             : //}
+     189             : 
+     190             : /* vectorFromSkewMatrix() //{ */
+     191             : 
+     192             : inline bool vectorFromSkewMatrix(const Eigen::Matrix3d& vec_skew, Eigen::Vector3d* vec) {
+     193             :   assert(vec);
+     194             :   if ((vec_skew + vec_skew.transpose()).norm() < kSmallValueCheck) {
+     195             :     *vec << vec_skew(2, 1), vec_skew(0, 2), vec_skew(1, 0);
+     196             :     return true;
+     197             :   } else {
+     198             :     std::cerr << "[eth_mav_msgs] Matrix is not skew-symmetric." << std::endl;
+     199             :     *vec = Eigen::Vector3d::Zero();
+     200             :     return false;
+     201             :   }
+     202             : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* isRotationMatrix() //{ */
+     207             : 
+     208             : inline bool isRotationMatrix(const Eigen::Matrix3d& mat) {
+     209             :   // Check that R^T * R = I
+     210             :   if ((mat.transpose() * mat - Eigen::Matrix3d::Identity()).norm() > kSmallValueCheck) {
+     211             :     std::cerr << "[eth_mav_msgs::common] Rotation matrix requirement violated (R^T * R = I)" << std::endl;
+     212             :     return false;
+     213             :   }
+     214             :   // Check that det(R) = 1
+     215             :   if (mat.determinant() - 1.0 > kSmallValueCheck) {
+     216             :     std::cerr << "[eth_mav_msgs::common] Rotation matrix requirement violated (det(R) = 1)" << std::endl;
+     217             :     return false;
+     218             :   }
+     219             :   return true;
+     220             : }
+     221             : 
+     222             : //}
+     223             : 
+     224             : /* matrixFromRotationVector() //{ */
+     225             : 
+     226             : // Rotation matrix from rotation vector as described in
+     227             : // "Computationally Efficient Trajectory Generation for Fully Actuated Multirotor Vehicles"
+     228             : // Brescianini 2018
+     229             : inline void matrixFromRotationVector(const Eigen::Vector3d& vec, Eigen::Matrix3d* mat) {
+     230             :   // R = (I + sin||r|| / ||r||) [r] + ((1 - cos||r||)/||r||^2) [r]^2
+     231             :   // where [r] is the skew matrix of r vector
+     232             :   assert(mat);
+     233             :   double          r_norm        = vec.norm();
+     234             :   Eigen::Matrix3d vec_skew_norm = Eigen::Matrix3d::Zero();
+     235             :   if (r_norm > 0.0) {
+     236             :     skewMatrixFromVector(vec / r_norm, &vec_skew_norm);
+     237             :   }
+     238             : 
+     239             :   *mat = Eigen::Matrix3d::Identity() + vec_skew_norm * std::sin(r_norm) + vec_skew_norm * vec_skew_norm * (1 - std::cos(r_norm));
+     240             : }
+     241             : 
+     242             : //}
+     243             : 
+     244             : /* vectorFromRotationMatrix() //{ */
+     245             : 
+     246             : // Rotation vector from rotation matrix as described in
+     247             : // "Computationally Efficient Trajectory Generation for Fully Actuated Multirotor Vehicles"
+     248             : // Brescianini 2018
+     249             : inline bool vectorFromRotationMatrix(const Eigen::Matrix3d& mat, Eigen::Vector3d* vec) {
+     250             :   // [r] = phi / 2sin(phi) * (R - R^T)
+     251             :   // where [r] is the skew matrix of r vector
+     252             :   // and phi satisfies 1 + 2cos(phi) = trace(R)
+     253             :   assert(vec);
+     254             : 
+     255             :   if (!isRotationMatrix(mat)) {
+     256             :     std::cerr << "[eth_mav_msgs::common] Not a rotation matrix." << std::endl;
+     257             :     return false;
+     258             :   }
+     259             : 
+     260             :   if ((mat - Eigen::Matrix3d::Identity()).norm() < kSmallValueCheck) {
+     261             :     *vec = Eigen::Vector3d::Zero();
+     262             :     return true;
+     263             :   }
+     264             : 
+     265             :   // Compute cosine of angle and clamp in range [-1, 1]
+     266             :   double cos_phi         = (mat.trace() - 1.0) / 2.0;
+     267             :   double cos_phi_clamped = boost::algorithm::clamp(cos_phi, -1.0, 1.0);
+     268             :   double phi             = std::acos(cos_phi_clamped);
+     269             : 
+     270             :   if (phi < kSmallValueCheck) {
+     271             :     *vec = Eigen::Vector3d::Zero();
+     272             :   } else {
+     273             :     Eigen::Matrix3d vec_skew = (mat - mat.transpose()) * phi / (2.0 * std::sin(phi));
+     274             :     Eigen::Vector3d vec_unskewed;
+     275             :     if (vectorFromSkewMatrix(vec_skew, &vec_unskewed)) {
+     276             :       *vec = vec_unskewed;
+     277             :     } else {
+     278             :       return false;
+     279             :     }
+     280             :   }
+     281             :   return true;
+     282             : }
+     283             : 
+     284             : //}
+     285             : 
+     286             : /* omegaFromRotationVector() //{ */
+     287             : 
+     288             : // Calculates angular velocity (omega) from rotation vector derivative
+     289             : // based on formula derived in "Finite rotations and angular velocity" by Asher
+     290             : // Peres
+     291             : inline Eigen::Vector3d omegaFromRotationVector(const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel) {
+     292             :   double phi = rot_vec.norm();
+     293             :   if (std::abs(phi) < 1.0e-3) {
+     294             :     // This captures the case of zero rotation
+     295             :     return rot_vec_vel;
+     296             :   }
+     297             : 
+     298             :   double phi_inv   = 1.0 / phi;
+     299             :   double phi_2_inv = phi_inv / phi;
+     300             :   double phi_3_inv = phi_2_inv / phi;
+     301             : 
+     302             :   // Create skew-symmetric matrix from rotation vector
+     303             :   Eigen::Matrix3d phi_skew;
+     304             :   skewMatrixFromVector(rot_vec, &phi_skew);
+     305             : 
+     306             :   // Set up matrix to calculate omega
+     307             :   Eigen::Matrix3d W;
+     308             :   W = Eigen::MatrixXd::Identity(3, 3) + phi_skew * (1 - std::cos(phi)) * phi_2_inv + phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv;
+     309             :   return W * rot_vec_vel;
+     310             : }
+     311             : 
+     312             : //}
+     313             : 
+     314             : /* omegaDotFromRotationVector() //{ */
+     315             : 
+     316             : // Calculates angular acceleration (omegaDot) from rotation vector derivative
+     317             : // based on formula derived in "Finite rotations and angular velocity" by Asher
+     318             : // Peres
+     319             : inline Eigen::Vector3d omegaDotFromRotationVector(const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel, const Eigen::Vector3d& rot_vec_acc) {
+     320             :   double phi = rot_vec.norm();
+     321             :   if (std::abs(phi) < 1.0e-3) {
+     322             :     // This captures the case of zero rotation
+     323             :     return rot_vec_acc;
+     324             :   }
+     325             : 
+     326             :   double phi_dot = rot_vec.dot(rot_vec_vel) / phi;
+     327             : 
+     328             :   double phi_inv   = 1.0 / phi;
+     329             :   double phi_2_inv = phi_inv / phi;
+     330             :   double phi_3_inv = phi_2_inv / phi;
+     331             :   double phi_4_inv = phi_3_inv / phi;
+     332             : 
+     333             : 
+     334             :   // Create skew-symmetric matrix from rotation vector and velocity
+     335             :   Eigen::Matrix3d phi_skew;
+     336             :   Eigen::Matrix3d phi_dot_skew;
+     337             : 
+     338             :   skewMatrixFromVector(rot_vec, &phi_skew);
+     339             :   skewMatrixFromVector(rot_vec_vel, &phi_dot_skew);
+     340             : 
+     341             :   // Set up matrices to calculate omega dot
+     342             :   Eigen::Matrix3d W_vel;
+     343             :   Eigen::Matrix3d W_acc;
+     344             :   W_vel = phi_skew * (phi * std::sin(phi) - 2.0f + 2.0f * std::cos(phi)) * phi_dot * phi_3_inv +
+     345             :           phi_skew * phi_skew * (-2.0f * phi - phi * std::cos(phi) + 3.0f * std::sin(phi)) * phi_dot * phi_4_inv +
+     346             :           phi_dot_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv;
+     347             : 
+     348             :   W_acc = Eigen::MatrixXd::Identity(3, 3) + phi_skew * (1.0f - std::cos(phi)) * phi_2_inv + phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv;
+     349             : 
+     350             :   return W_vel * rot_vec_vel + W_acc * rot_vec_acc;
+     351             : }
+     352             : 
+     353             : //}
+     354             : 
+     355             : /* getSquaredRotorSpeedsFromAllocationAndState() //{ */
+     356             : 
+     357             : // Calculate the nominal rotor rates given the MAV mass, allocation matrix,
+     358             : // angular velocity, angular acceleration, and body acceleration (normalized
+     359             : // thrust).
+     360             : //
+     361             : // [torques, thrust]' = A * n^2, where
+     362             : // torques = J * ang_acc + ang_vel x J
+     363             : // thrust = m * norm(acc)
+     364             : //
+     365             : // The allocation matrix A has of a hexacopter is:
+     366             : // A = K * B, where
+     367             : // K = diag(l*c_T, l*c_T, c_M, c_T),
+     368             : //     [ s  1  s -s -1 -s]
+     369             : // B = [-c  0  c  c  0 -c]
+     370             : //     [-1  1 -1  1 -1  1]
+     371             : //     [ 1  1  1  1  1  1],
+     372             : // l: arm length
+     373             : // c_T: thrust constant
+     374             : // c_M: moment constant
+     375             : // s: sin(30°)
+     376             : // c: cos(30°)
+     377             : //
+     378             : // The inverse can be computed computationally efficient:
+     379             : // A^-1 \approx B^pseudo * K^-1
+     380             : inline void getSquaredRotorSpeedsFromAllocationAndState(const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia, double mass,
+     381             :                                                         const Eigen::Vector3d& angular_velocity_B, const Eigen::Vector3d& angular_acceleration_B,
+     382             :                                                         const Eigen::Vector3d& acceleration_B, Eigen::VectorXd* rotor_rates_squared) {
+     383             :   const Eigen::Vector3d torque       = inertia.asDiagonal() * angular_acceleration_B + angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B);
+     384             :   const double          thrust_force = mass * acceleration_B.norm();
+     385             :   Eigen::Vector4d       input;
+     386             :   input << torque, thrust_force;
+     387             :   *rotor_rates_squared = allocation_inv * input;
+     388             : }
+     389             : 
+     390             : //}
+     391             : 
+     392             : /* nanosecondsToSeconds() //{ */
+     393             : 
+     394             : inline double nanosecondsToSeconds(int64_t nanoseconds) {
+     395             :   double seconds = nanoseconds / kNumNanosecondsPerSecond;
+     396             :   return seconds;
+     397             : }
+     398             : 
+     399             : //}
+     400             : 
+     401             : /* secondsToNanoseconds() //{ */
+     402             : 
+     403             : inline int64_t secondsToNanoseconds(double seconds) {
+     404             :   int64_t nanoseconds = static_cast<int64_t>(seconds * kNumNanosecondsPerSecond);
+     405             :   return nanoseconds;
+     406             : }
+     407             : 
+     408             : //}
+     409             : 
+     410             : }  // namespace eth_mav_msgs
+     411             : 
+     412             : #endif  // eth_mav_msgs_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html new file mode 100644 index 0000000000..f59c0f0674 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html @@ -0,0 +1,123 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..380603cff091ec67f270e981cf960298d7adee18 GIT binary patch literal 1579 zcmV+`2Gse9P)Kp2s@!4lVLUZye=h&5IK;y<1p9I5C53HMK!-|p05 z8Xphp44i*;z;%)|WO+lr02B$35l>>bNSgJNI75{Wh#pX1fYnrPq7e`AE0UT={@Gg0Z9pBap(4M1i5@ z%J&dQ^EZG;IG+W4Re>AJ$5@XO6@iMU@|i*?fCp8;2S5X3?!3bl-V&P6v~MRy>pf(z zp@t;YdyDZ>a-^L7Pr%rdqkr>AnYjJ4ay&mawN8T)ruusa z;I&kEQbcJ~h&n;F+W1xBIT&4jJHrnfFC>?FljBLE>_psN3|>r{60pG%+yRv9VW9jR z;Kf378LuYeu2E#fWNWGAG#uc45pX{96;=3pHFcmu-%3mC<~TF%i>Gz%WiO?yvH|p& z^ScFYAehJfO`Msl8O7VnYqXcD>M2vAKh}k5ahrR;0`CaCSR^WN&rPxPL1bT#H?pmC>U`4*5}@ZK zeU!wEt8hHrI}R&Uk%`~R?Zn`(&-~3kqX{N^&w!hl&+yr--5tdkI*=Wse{nb+4jm`U z!RHxW_kf<-ptJK#cr<;U%C)qzSgiKuQ@@89ae&+u&Zb?r+KQd-mxU?BZtKDrbR(s5 zn#cg=%L?!XW`;$KB{HU&<^5~Ua!N<`Vm)tU0fz?M^tnW=$J~r*T;k?4%a0Fw!WFvL zz@{U~$L27#0Cwa8E5(O7S)KfRi%EO)ZrwLaEVfA!vnLkm+F)aM6)yAM6&_(0a(;db zvc$*Y?o+^05e`5dV6aI%p;4@a4TdvoJGE$Zdc;@9ado{!BcEC)n=Ou}o|l77?}(Qo z@-~I)W4)Z||_5P&QYwM=D|82*96>J?_v8{pYdAmo&#KhB(GgM6)p9`Qtb~ zRzFs;#utf?^I|VfW5@ivcq>N{#-1mFL$is^!>Q-nqzi!3KjNs(%MZUH##r;3vFx6i zPHp*yonAn}#)Nh6>FiQy(@SwLVKi$^gQdoC!GxqTdb%vGvI;-=WK+pM3|N3AgMc<- zfYt4td^Ghcjg&SP25XxVz&VW4Vdp*tCLI4DW|>A2*X}9&+mZJC%1$2)N-_(`k{vO< z)A-;M0kAV&gYf~l!;S&>UsZ0H9#cTb7{}JmKldCVD}Yi4jRmv-I9Kx`;1oh>gyNm* zB3k1)KwXcHk~I;vtxp+R$BoH)4$x%3tjYGLQaJ>eTvWacKB^mTSQKO1a3$~vQs;1a zsDZm$us`u+L2W+VhwMPwtl4VM0#2F#p9NlJZD_@vAhGtOuIyD~ dx{ox(`3KrNS#m3rBFX>&002ovPDHLkV1fxT^?v{W literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html new file mode 100644 index 0000000000..aad49e3407 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - eigen_mav_msgs.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2424100.0 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_mav_msgs::EigenTrajectoryPoint::EigenTrajectoryPoint()2067
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html new file mode 100644 index 0000000000..9043a5af63 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - eigen_mav_msgs.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2424100.0 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_mav_msgs::EigenTrajectoryPoint::EigenTrajectoryPoint()2067
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html new file mode 100644 index 0000000000..af395bc31c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html new file mode 100644 index 0000000000..c5fa9af027 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html @@ -0,0 +1,463 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - eigen_mav_msgs.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2424100.0 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..26d81d99e693319d43cb18eb4ec44b3db73e1fd9 GIT binary patch literal 1483 zcmV;+1vL7JP)gb=KIi{O6P4+ovhDATXcEIumWTt!*g?bXbV=C@( zzUr*a*}`=_7=vAOr0OtmhIQ7%VBr!lPVqK12^j-|Mk!*1*zKm?ivIz+{AtcZ5#lnw zNXDFZ2ZnHA*3m%=ID5Ad%BCnv=kb6udSn5cgAS-gtBfm$$#E)v4DNsRzZokZ20oU1I6F~0{Ad-)y#4qW_?Ed?Np-O{QF%g9|WZt3Bk z!}2XE{YCR?MDgHH%_`s=Je?h`(!$s&0v3o#_@qcQJl4@!^CH2q-v()>HkxVe$UVNb zw}K*jtf}EhT|_iAHNjHJFfH!afd*6qs^B2Zq}W)0VGM7QW|1 zg(;v9?i3gWnpnd!O($=WWlFU=LLT5ztjN`G{)jRHU==!NfmwZ6#}o+#nF62$0`kWK z4CoUoKf^YhAve~c4i!CgwgHo3`^>uKid%nJb`Q{o7JC(Y$0%Af6`&p)ZRszL<+Z{M zNm7yxI zSOF>3&FVhIhyBqx$^^b_1QsH8+W6TfAeIO4tc-c%gS@_=1v)ecr?e?>^fTcNjoH+sCXgzxjc+la(Ib0S6?Y<%a+sZ=RI>|!cO z@g3dFdp**%`!<6D)? zJRWAUc^#;^*qf7isB>vpJ5s&_)bsIW+1|oFh0}bf`xFu2amVRWWMIA)z?R-55G!Dh zF(;!!Ws8J>b;o9MBqf2?M!0)@O$?9Xl8T*emfkow;X@ zuT=PZ6uTVuW2aKGV6O`C?)Xa4t6Ur1Q9;qeFQ~8OV@j3Xx|u_@wy$GsjUrTA+3_bc zDo|%HwF{oJ>%eywc(f#y#o0J@?N7@U++1DQ_J%mV$u-uol~5tWrIPg1)o`#RZPbH~ zkOUbWI$yR-PJ3+DhZ?o7DZdM}RlX!hRheP$hZ@p5T)e&%+J-6%A(_@9)I$8lG9^bT zWRFY~*-9*~) + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2828100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
<unnamed>100.0 %24 / 24100.0 %1 / 1
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html new file mode 100644 index 0000000000..c7c8722bde --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2828100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
<unnamed>100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html new file mode 100644 index 0000000000..b51cc3f1e6 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2828100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
<unnamed>100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html new file mode 100644 index 0000000000..c8b59f2ae6 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2828100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html new file mode 100644 index 0000000000..e4c96cb494 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2828100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html new file mode 100644 index 0000000000..474c41c0a8 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2828100.0 %
Date:2024-01-07 21:47:14Functions:33100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
common.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
eigen_mav_msgs.h +
100.0%
+ 		100.0 %24 / 24100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html new file mode 100644 index 0000000000..33f672efeb --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - extremum.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html new file mode 100644 index 0000000000..3ae4b93aba --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - extremum.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html new file mode 100644 index 0000000000..83207571c1 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html new file mode 100644 index 0000000000..a66210474a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html @@ -0,0 +1,143 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - extremum.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..42a2d6ec4d0c683918ea2dd366e04e84d905a8f2 GIT binary patch literal 437 zcmV;m0ZRUfP)}djTrb8O;8fO7I^n`rmt^V#vdcju9D$0wQ)BB=PsMmKbbvD+ zO+8TU@*;2r^g!(gLp&@qoQi^i_|%RGE>V?7>&#QsRv~@rPQC=*?)=ugoIVLY44k2+ zet&Rs@raS|UY9YnV-+uodfdC(AyB-$9z5!EEw~LQGc0x7vMoy<*R74|x45?u*EnRO z5mss=Tx?n^`^q`T^*_|h))>;TX3nd}7!5*!?x}}k{37+ZSwZGE!M<5(Yp_=7ta<|B zI60Sl!dU0S9(eRza1&9^ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31462550.2 %
Date:2024-01-07 21:47:14Functions:193259.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polynomial_optimization_nonlinear_impl.h +
35.4%35.4%
+ 		35.4 %146 / 41344.4 %8 / 18
<unnamed>35.4 %146 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
<unnamed>79.2 %168 / 21278.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html new file mode 100644 index 0000000000..f921e63f31 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31462550.2 %
Date:2024-01-07 21:47:14Functions:193259.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polynomial_optimization_nonlinear_impl.h +
35.4%35.4%
+ 		35.4 %146 / 41344.4 %8 / 18
<unnamed>35.4 %146 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
<unnamed>79.2 %168 / 21278.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html new file mode 100644 index 0000000000..ca1c98f0ef --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31462550.2 %
Date:2024-01-07 21:47:14Functions:193259.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
<unnamed>79.2 %168 / 21278.6 %11 / 14
polynomial_optimization_nonlinear_impl.h +
35.4%35.4%
+ 		35.4 %146 / 41344.4 %8 / 18
<unnamed>35.4 %146 / 41344.4 %8 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html new file mode 100644 index 0000000000..2998793f53 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31462550.2 %
Date:2024-01-07 21:47:14Functions:193259.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polynomial_optimization_nonlinear_impl.h +
35.4%35.4%
+ 		35.4 %146 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html new file mode 100644 index 0000000000..4489a4d29c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31462550.2 %
Date:2024-01-07 21:47:14Functions:193259.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polynomial_optimization_nonlinear_impl.h +
35.4%35.4%
+ 		35.4 %146 / 41344.4 %8 / 18
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html new file mode 100644 index 0000000000..71f0bd5b79 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31462550.2 %
Date:2024-01-07 21:47:14Functions:193259.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
polynomial_optimization_linear_impl.h +
79.2%79.2%
+ 		79.2 %168 / 21278.6 %11 / 14
polynomial_optimization_nonlinear_impl.h +
35.4%35.4%
+ 		35.4 %146 / 41344.4 %8 / 18
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html new file mode 100644 index 0000000000..0dd6a135c3 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_linear_impl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16821279.2 %
Date:2024-01-07 21:47:14Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimization<10>::setFreeConstraints(std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > > const&)0
eth_trajectory_generation::PolynomialOptimization<10>::computeSegmentMaximumMagnitudeCandidates(int, eth_trajectory_generation::Segment const&, double, double, std::vector<double, std::allocator<double> >*)0
eth_trajectory_generation::PolynomialOptimization<10>::computeMaximumOfMagnitude(int, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> >*) const0
eth_trajectory_generation::PolynomialOptimization<10>::setupFromVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> > const&, int)14
eth_trajectory_generation::PolynomialOptimization<10>::setupConstraintReorderingMatrix()14
eth_trajectory_generation::PolynomialOptimization<10>::PolynomialOptimization(unsigned long)14
eth_trajectory_generation::PolynomialOptimization<10>::computeCost() const1555
eth_trajectory_generation::PolynomialOptimization<10>::solveLinear()1712
eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentsFromCompactConstraints()1712
eth_trajectory_generation::PolynomialOptimization<10>::constructR(Eigen::SparseMatrix<double, 0, int>*) const1712
eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentTimes(std::vector<double, std::allocator<double> > const&)1726
eth_trajectory_generation::PolynomialOptimization<10>::setupMappingMatrix(double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)23676
eth_trajectory_generation::PolynomialOptimization<10>::invertMappingMatrix(Eigen::Matrix<double, 10, 10, 0, 10, 10> const&, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)23676
eth_trajectory_generation::PolynomialOptimization<10>::computeQuadraticCostJacobian(int, double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)23676
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html new file mode 100644 index 0000000000..e4a483e4fd --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_linear_impl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16821279.2 %
Date:2024-01-07 21:47:14Functions:111478.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..d2bf04eef3eaca36f74c3ea7739ee1959089d779 GIT binary patch literal 2760 zcmV;(3ODtMP)WD5Wqft1nR4vXzhq1jr=v56I3|EayI~Edm_zG zxKAju{tX%$*)wXRRy<5m0n|}cm#R`|n$o41Xl{!MmyfL_>4|CB#yN@`P6H$0-YG8p ze+y-~-JE!IYZj_qHSE>aU=}#m1f9-2J0yB-+r7T+^`Zs30$6<0tXd14vbw4w6xUm{ zD=6v}*ESW`ifhhr>zw>$yjkw|`{Q_=kLTax;r~D30~UW1ZV~wW3{Q{vC|sTt4u3g4 z@D5-2XXp8dpMm5Zik>~PGfoc3Oj`g*U%IM>rTWOgx7F;HwN3|oUfRI9s)cZcHoQrZ z6veBL!&@6J{{Q{uHgZt_TO!m}z3_^J{7l35IHBUmBQIpEk1q1r(p513$uDdper0X9 zKNFn+U&??3W!je9SAApQHJHe4lvbq!lw%KzjVf;)+MXxUQpJfR(^dpa0cIwcj(e*} z;0YE7O%q$IGdx-1Zw@d|N3cK#{`4SQy3n`%=Wly*CGA z)ltX#K;8VRclT1>^r|kH*a6yZHxQ_H71ZUUuEHfA?nSNwJwjK(TXQR>4U0qBe+Skc z_}E{5XWd>`vEFa^!oi~0rwR4NSJjXy7%4Dfg$;5-T@Yvk%_*f8mO~Sv&gZIa(>9z+t8{TNGSYu#i;!@(|pmm`IKo6H!l2P zoy+<7G`CR3-TqZT<&?TXpxX>ZvxQ2{cu0g$=F0KQZR1FH)~IbwH%*q-cmtsCHZM!6 zz}t?hAk9ME0BA=c?_h8+mbL*zw=h1dashWTkZjki)V;R|*m zUS>EdZnhRLD}POKd!eqhGkY6Z2fh^|sVvw1=y$Z52`#sC2A~=5q0vKajP{`y@|lhI zaHIV7)VL5N0YV))q3*oA=vTuUa!H`LUp#1FpuYi?E3a7t3A?wkGUu15?-Ot?8py z`oNY>ykc>o{W;03Gg&o_X>P9SaqwXlxC;|W3@L#PWDdm$yLCM)LBOgMVFq*=kQ`;p zlUup8ze$q*qkJkX8v$LcQB*JX=9pr&D>=n$4FFWEps0?ev_zOQQiyO!Q7^&|bVi}z zI0{u1wNcj>3~9ZP1pd}D{W4t7pbjUCilF3H03vwO;woaPPR4OzMO*Qtuxbe`HzJYZ zOdCnhQae@$It<$lDC(=u4Rh61OJmIe753~4E;WFA4z%R3C)Mncoi44qU~j$GvIY@+ zDCX;{qt{<%0RBt+g+eiGZUCfrmhmysFmbgMKZ8&69;p?psrN*v99z+(#FUD96H<>9 zQz9IY#oog<&kMGvEtzm(U@t1%plDmeHNm7M<)gAQUo;9Y8i9f0h0|~tG-5k-#K%3I zw5H>ac{Mec%ei7s_~AngSZD&Q_xI|IS15Q_qDroKv} z;MEY>TmnRyTHeyQI0xy)L?D;64cQg0Lx-JyKDT{kjNjceh7P8{05)Pi4a0Edfrz2b zR%gmEhFp$w_Uso~v!6*0spp22?8)VClbjG<6NDXc@UD4v&|tWzI)ss145!RO$QQ&E zWtrTJSjQsQYEE*$pR7l+O z-h=8GQNQXHvvAj_p0o$OCBQJW<1yWp{8#x#;@L4<8WBwIyh z)^zz*B7DfxTg`IHqY|AG+!_AW$V+*I^B8cR<2d3uvV+cE91j30OFR!u139MHLGgn= za=bqCCWSjAxm5Mda)$ATvjp~7P6t*Nr4KjZ#QiJ9eMfsgZ+kl63Bx39Om>Y2#8R4= zJ2S4i+Z#%@oH@EX)^knaB%rt~rs;A`oNRD#%kL{~1WKvPP-$)rSViVQ!^`_u89<_y zea5hXF@{|EST&>$cPY|ufznDxUxKkJ>ek455nWXSx2<;|kVnmBtW1WX$p@vRDLI)+ z+oXm;5b@Yf{&!N@X2|)2_w~GJT-AEY6G+*SNm!PKusmE9S*TZRwpPLG1z(DV~Bi&13iB6(f&$z@K=;YcKkQ?;?4? zdjmRe4&WCD3~={IaiEdnRRpqBfw~1>WcU!M-UVO>U^Nsrsg{)8si@`6qGLCAW{A7L zW&qmko-+WdVSWF$75l%=80wk&Q+zICDLA|$T(Xic!d0P%D#cs^DvlOft-3sJm!%Ism9t`i8@81p`93P)A7;E9d^F zRUq}>r)HvyZ!4Ty%|_|-c$h!QiL<>#trt&AcVEjRlfLOauzQ&mn + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_nonlinear_impl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:14641335.4 %
Date:2024-01-07 21:47:14Functions:81844.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTime()0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTime(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeAndFreeConstraints()0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::evaluateMaximumMagnitudeConstraint(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeAndConstraints(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::setFreeEndpointDerivativeHardConstraints(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> >*, std::vector<double, std::allocator<double> >*)0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::evaluateMaximumMagnitudeAsSoftConstraint(std::vector<std::shared_ptr<eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::ConstraintData>, std::allocator<std::shared_ptr<eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::ConstraintData> > > const&, double, double) const0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#2}::operator()(double) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#3}::operator()(double) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#1}::operator()(double) const [clone .isra.0]0
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::setupFromVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> > const&, int)14
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()14
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::scaleSegmentTimesWithViolation()14
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimize()14
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::PolynomialOptimizationNonLinear(unsigned long, eth_trajectory_generation::NonlinearOptimizationParameters const&)14
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getCostAndGradientMellinger(std::vector<double, std::allocator<double> >*)143
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeMellingerOuterLoop(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)143
eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::addMaximumMagnitudeConstraint(int, int, double)168
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html new file mode 100644 index 0000000000..aafedcf015 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_nonlinear_impl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:14641335.4 %
Date:2024-01-07 21:47:14Functions:81844.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..74124676d797f0c42e57c38890aab169b9a5f47b GIT binary patch literal 3527 zcmV;&4LI_NP)TcC@7}Mc4xnF_nk@GrwE9sh|Vw%#xjoc zS;q4}_y6QxO!2k zA~~_@KK0~f5`aAvy_!e=(=j}SSD=BJRXt*B1pE{vn^?=tpb-3(+H*n`u_naYM*QKM zPpVpwHv-j9t`^{`RqVi?s_W9`zz*70iY-B8;3usx=Ze&b-QLtgl7)&CY&P-adhi^~ ze+0mYrY?}2Cd8c|Pn3AJesX$>B?*>LlLqawHdl>e5A#2t!=OJnJ?#xT+O#R;J>!YE|xw zK*waohZ7zhNy?aBwF4T|vu*FQY|U~4Jrc@sr00DnA4H&q;?V|qpt+!OG_9lc$e#;( z{8s`QxC&euMPoTBNp;d#_A58~q1_1zinXp4fUdGVFqL=GTdG#( z0VnYAl|rq%N>P@EmD7(rF@GHL(YAsd=P?AzFj`20ZBrVXZ7Wh)YC*uJdJYq?SK+SM z6}}~#e(#6=EvR~rM!Dm&{~4YgvjNx~;9OhZ?ntrbqwL^Wzdp9T&R*ovT&(se53akw z<3G}bURUnvh??8J+l0R&FrSXwG8_{EroVmOSDt;D;_s zKU|Vh+5imp2YG(4jy`(<>Hxi^ddmp;t{Ee_JJ6)XS#Kn0)v(&{w=1*Nut=`yuu^anc9cp zzMG*@A3tJ&amG&_;YlC4W?Vz*qx%C(`#>tosgR}$Rwq@AuB$xDcFsOc#LRxcHujIO9F-W5OY|o*fK;ur8@~HYh%_an4Og8b%-p5GHTJNoK znPTwwUtOjcRw+wwfUS~+pGu6IH$xxAGDU0A3p} zcob*LPXg^ck}aIf*)I&p7O(ou6sFgS3Lld3YhjKHApC~BR#-DSYte(3TPZFHXs2jX zcu}*P=HH~p8jIKT_#s6>k8O%y*CXi$a@E~>{FGv;M*~HZ9_<711AFYo%_fT=QJ6H_ zh7}oERi&jyGjDIlO%ja59r(U`nQfM&-4uw%DO)=B^w36@a}+Iu=_REw4-d)=9jn>!Jq7V;U8~sDPKH^TseG^#6D_= zFYsXpJsPF{;^7MjXf)$9Tb(skm|gphB6W!zHs%rzA&=wmnU-kb=jR8Ggn@}@ywEbY z4{@|*fT>tF^JkwJg}izAshN3JZNSpZ?gv_h8Bzc@WfjXUbM@4a3VgtNsPQCT^GwN@ zaen&Vh&dE_q>XNo>yC}?=Qgim)Oe(4188Fw9+xPKiUqxJE2L)!Z`?4f*rPa2w;=wEY?nI#I_izhog!$Jv@s8otlN^^W6%poy*QNG}`e{z0oKNYZ zXwlA7!v|bWh29gAAHuGCoc7E43uVa%4f2>tom+|my$ARv=HBCRdrp29)r)QYj{t@8 zcW$_<&aa;vewSlaDyVQH4K5YxyJr5ilFx~+^yNXh&_YI>*&N8}MtHKe;@ z?MKP1RqJ8y^1i)B25AIt#t}*MW+Kqsu{WdhK^`;bgRw3cz~I89hIjP+`fHn?F?ju@ znvId+)&*5j^=uM%kT%&d_;?h+ddsw2astXM`3E{QU#F<4;EwoXWV8;HmZU*}IKIl0 zkb?u=H?>7gPElR`@KC&`D3#tOY}whqZQZxJiGgTV`fhbQf$!S-U#(i0#eVg!&#k5I zgIrOEM{QvzK{!mBfLr`N*1-417!Xj>_SrY2j&hUfwQf04YAcTq%yE00aHCBZgXm<~ zvMr@Rl~kv$|;3`Vg5L~&Z%E@*HK*L)C)*0!eJY~Rz*SYrZyb@gFAr)i&&(#dr= zP6Y=$$*qYC1vV&Nsya-9(`@*2VJf=%gvX5II#9WvVo^V4z{Gy@3}mS_NovbEve%1T zaV!mhq)R?`^0T7xmGam-#8qy`g3GZE>H?&Ib9PmWe*23(*4%%a1RoWOFFtwEc*D%A zKpI!LcF%CN&wTSL)b?CB+9T~l|A93ZTU02kig%H2qIs4CggR7Uzts7pe$S84cT-Gc zU1yFCho%YkQg>hDd1XZW+=hY9NIH-K)y(Fr!%8M!x7DwTMezwN_fO z6e{%du@qKYp`N1ZzgWs&Eam3%7fTUsrDf`g&u{$-@4o4b%$Pti@V6tghZx=H zU8Yb);P1RE!1ljxefOMh;10R@7bisC3;407K_&ng31Za1Iv`yFl~xobpWRRf8CMZ4}?UG z*us~99m4RykQnhf2rzy5;1Ejj*ofApk*tSDJOKfJbsBFD!W`b}d^ zPXR-E)6D+(wYS`u%nfKvg_^QICGP*q_LMS~<6Uj>;?$W_Bxg9u*=f9!ZK6Op2Bl`v z;PIlpR1poy$8lz7aYfn8xK-rF!xv`=EUk?Hts$`7qX5#ig0%_7z>pB^@~$}?Rq*aM zJAs@u}X>}m_WNukF7Kt!28qjrj#y<<+EDA&9 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:12018564.9 %
Date:2024-01-07 21:47:14Functions:101376.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
<unnamed>58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
<unnamed>100.0 %7 / 7-0 / 0
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
<unnamed>75.0 %6 / 8-0 / 0
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
<unnamed>100.0 %6 / 6-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
<unnamed>100.0 %13 / 13100.0 %1 / 1
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
<unnamed>65.5 %19 / 29100.0 %1 / 1
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
<unnamed>91.8 %45 / 49100.0 %6 / 6
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html new file mode 100644 index 0000000000..4bd6bceebf --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:12018564.9 %
Date:2024-01-07 21:47:14Functions:101376.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
<unnamed>58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
<unnamed>65.5 %19 / 29100.0 %1 / 1
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
<unnamed>75.0 %6 / 8-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
<unnamed>91.8 %45 / 49100.0 %6 / 6
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
<unnamed>100.0 %6 / 6-0 / 0
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
<unnamed>100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
<unnamed>100.0 %13 / 13100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html new file mode 100644 index 0000000000..3c49c14675 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:12018564.9 %
Date:2024-01-07 21:47:14Functions:101376.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
<unnamed>100.0 %6 / 6-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
<unnamed>91.8 %45 / 49100.0 %6 / 6
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
<unnamed>65.5 %19 / 29100.0 %1 / 1
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
<unnamed>100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
<unnamed>100.0 %13 / 13100.0 %1 / 1
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
<unnamed>58.5 %24 / 4166.7 %2 / 3
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
<unnamed>75.0 %6 / 8-0 / 0
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html new file mode 100644 index 0000000000..dd20a5a885 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:12018564.9 %
Date:2024-01-07 21:47:14Functions:101376.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html new file mode 100644 index 0000000000..2260cd1760 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:12018564.9 %
Date:2024-01-07 21:47:14Functions:101376.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html new file mode 100644 index 0000000000..91e94bebfc --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:12018564.9 %
Date:2024-01-07 21:47:14Functions:101376.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
extremum.h +
100.0%
+ 		100.0 %6 / 6-0 / 0
polynomial.h +
91.8%91.8%
+ 		91.8 %45 / 49100.0 %6 / 6
polynomial_optimization_linear.h +
65.5%65.5%
+ 		65.5 %19 / 29100.0 %1 / 1
polynomial_optimization_nonlinear.h +
100.0%
+ 		100.0 %7 / 7-0 / 0
segment.h +
100.0%
+ 		100.0 %13 / 13100.0 %1 / 1
timing.h +
0.0%
+ 		0.0 %0 / 320.0 %0 / 2
trajectory.h +
58.5%58.5%
+ 		58.5 %24 / 4166.7 %2 / 3
vertex.h +
75.0%75.0%
+ 		75.0 %6 / 8-0 / 0
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html new file mode 100644 index 0000000000..7d0795437b --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:454991.8 %
Date:2024-01-07 21:47:14Functions:66100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Polynomial::Polynomial(int)14
eth_trajectory_generation::Polynomial::evaluate(double, int) const64256
eth_trajectory_generation::Polynomial::Polynomial(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)93792
eth_trajectory_generation::Polynomial::getCoefficients(int) const96808
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double)236760
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double, Eigen::Matrix<double, -1, 1, 0, -1, 1>*)236760
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html new file mode 100644 index 0000000000..6a38a864c6 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:454991.8 %
Date:2024-01-07 21:47:14Functions:66100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double)236760
eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double, Eigen::Matrix<double, -1, 1, 0, -1, 1>*)236760
eth_trajectory_generation::Polynomial::Polynomial(int)14
eth_trajectory_generation::Polynomial::Polynomial(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)93792
eth_trajectory_generation::Polynomial::getCoefficients(int) const96808
eth_trajectory_generation::Polynomial::evaluate(double, int) const64256
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html new file mode 100644 index 0000000000..1c7c9ff042 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html new file mode 100644 index 0000000000..74b6937003 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html @@ -0,0 +1,353 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:454991.8 %
Date:2024-01-07 21:47:14Functions:66100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_POLYNOMIAL_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_POLYNOMIAL_H_
+      23             : 
+      24             : #include <eth_trajectory_generation/misc.h>
+      25             : #include <Eigen/Eigen>
+      26             : #include <Eigen/SVD>
+      27             : #include <utility>
+      28             : #include <vector>
+      29             : 
+      30             : namespace eth_trajectory_generation
+      31             : {
+      32             : 
+      33             : // Implementation of polynomials of order N-1. Order must be known at
+      34             : // compile time.
+      35             : // Polynomial coefficients are stored with increasing powers,
+      36             : // i.e. c_0 + c_1*t ... c_{N-1} * t^{N-1}
+      37             : // where N = number of coefficients of the polynomial.
+      38      106286 : class Polynomial {
+      39             : public:
+      40             :   typedef std::vector<Polynomial> Vector;
+      41             : 
+      42             :   // Maximum degree of a polynomial for which the static derivative (basis
+      43             :   // coefficient) matrix should be evaluated for.
+      44             :   // kMaxN = max. number of coefficients.
+      45             :   static constexpr int kMaxN = 12;
+      46             :   // kMaxConvolutionSize = max. convolution size for N = 12, convolved with its
+      47             :   // derivative.
+      48             :   static constexpr int kMaxConvolutionSize = 2 * kMaxN - 2;
+      49             :   // One static shared across all members of the class, computed up to order
+      50             :   // kMaxConvolutionSize.
+      51             :   static Eigen::MatrixXd base_coefficients_;
+      52             : 
+      53          14 :   Polynomial(int N) : N_(N), coefficients_(N) {
+      54          14 :     coefficients_.setZero();
+      55          14 :   }
+      56             : 
+      57             :   // Assigns arbitrary coefficients to a polynomial.
+      58       93792 :   Polynomial(int N, const Eigen::VectorXd& coeffs) : N_(N), coefficients_(coeffs) {
+      59       93792 :     CHECK_EQ(N_, coeffs.size()) << "Number of coefficients has to match.";
+      60       93792 :   }
+      61             : 
+      62        2736 :   Polynomial(const Eigen::VectorXd& coeffs) : N_(coeffs.size()), coefficients_(coeffs) {
+      63        1368 :   }
+      64             :   /// Gets the number of coefficients (order + 1) of the polynomial.
+      65             :   int N() const {
+      66             :     return N_;
+      67             :   }
+      68             : 
+      69           0 :   inline bool operator==(const Polynomial& rhs) const {
+      70           0 :     return coefficients_ == rhs.coefficients_;
+      71             :   }
+      72           0 :   inline bool operator!=(const Polynomial& rhs) const {
+      73           0 :     return !operator==(rhs);
+      74             :   }
+      75             :   inline Polynomial operator+(const Polynomial& rhs) const {
+      76             :     return Polynomial(coefficients_ + rhs.coefficients_);
+      77             :   }
+      78             :   inline Polynomial& operator+=(const Polynomial& rhs) {
+      79             :     this->coefficients_ += rhs.coefficients_;
+      80             :     return *this;
+      81             :   }
+      82             :   // The product of two polynomials is the convolution of their coefficients.
+      83             :   inline Polynomial operator*(const Polynomial& rhs) const {
+      84             :     return Polynomial(convolve(coefficients_, rhs.coefficients_));
+      85             :   }
+      86             :   // The product of a polynomial with a scalar. Note that polynomials are in
+      87             :   // general not homogeneous, i.e., f(a*t) != a*f(t)
+      88             :   inline Polynomial operator*(const double& rhs) const {
+      89             :     return Polynomial(coefficients_ * rhs);
+      90             :   }
+      91             : 
+      92             :   // Sets up the internal representation from coefficients.
+      93             :   // Coefficients are stored in increasing order with the power of t,
+      94             :   // i.e. c1 + c2*t + c3*t^2 ==> coeffs = [c1 c2 c3]
+      95             :   /* setCoefficients() //{ */
+      96             : 
+      97             :   void setCoefficients(const Eigen::VectorXd& coeffs) {
+      98             :     CHECK_EQ(N_, coeffs.size()) << "Number of coefficients has to match.";
+      99             :     coefficients_ = coeffs;
+     100             :   }
+     101             : 
+     102             :   //}
+     103             : 
+     104             :   // Returns the coefficients for the specified derivative of the
+     105             :   // polynomial as a ROW vector.
+     106             :   /* getCoefficients() //{ */
+     107             : 
+     108       96808 :   Eigen::VectorXd getCoefficients(int derivative = 0) const {
+     109       96808 :     CHECK_LE(derivative, N_);
+     110       96808 :     if (derivative == 0) {
+     111      185408 :       return coefficients_;
+     112             :     } else {
+     113       16416 :       Eigen::VectorXd result(N_);
+     114        8208 :       result.setZero();
+     115        8208 :       result.head(N_ - derivative) =
+     116        8208 :           coefficients_.tail(N_ - derivative).cwiseProduct(base_coefficients_.block(derivative, derivative, 1, N_ - derivative).transpose());
+     117        8208 :       return result;
+     118             :     }
+     119             :   }
+     120             : 
+     121             :   //}
+     122             : 
+     123             :   // Evaluates the polynomial at time t and writes the result.
+     124             :   // Fills in all derivatives up to result.size()-1 (that is, if result is a
+     125             :   // 3-vector, then will fill in derivatives 0, 1, and 2).
+     126             :   /* evaluate() //{ */
+     127             : 
+     128             :   void evaluate(double t, Eigen::VectorXd* result) const {
+     129             :     CHECK_LE(result->size(), N_);
+     130             :     const int max_deg = result->size();
+     131             : 
+     132             :     const int tmp = N_ - 1;
+     133             :     for (int i = 0; i < max_deg; i++) {
+     134             :       Eigen::RowVectorXd row = base_coefficients_.block(i, 0, 1, N_);
+     135             :       double             acc = row[tmp] * coefficients_[tmp];
+     136             :       for (int j = tmp - 1; j >= i; --j) {
+     137             :         acc *= t;
+     138             :         acc += row[j] * coefficients_[j];
+     139             :       }
+     140             :       (*result)[i] = acc;
+     141             :     }
+     142             :   }
+     143             : 
+     144             :   //}
+     145             : 
+     146             :   // Evaluates the specified derivative of the polynomial at time t and returns
+     147             :   // the result (only one value).
+     148             :   /* evaluate() //{ */
+     149             : 
+     150       64256 :   double evaluate(double t, int derivative) const {
+     151       64256 :     if (derivative >= N_) {
+     152             :       return 0.0;
+     153             :     }
+     154       64256 :     double             result;
+     155       64256 :     const int          tmp = N_ - 1;
+     156       64256 :     Eigen::RowVectorXd row = base_coefficients_.block(derivative, 0, 1, N_);
+     157       64256 :     result                 = row[tmp] * coefficients_[tmp];
+     158      506806 :     for (int j = tmp - 1; j >= derivative; --j) {
+     159      442550 :       result *= t;
+     160      442550 :       result += row[j] * coefficients_[j];
+     161             :     }
+     162       64256 :     return result;
+     163             :   }
+     164             : 
+     165             :   //}
+     166             : 
+     167             :   // Uses Jenkins-Traub to get all the roots of the polynomial at a certain
+     168             :   // derivative.
+     169             :   bool getRoots(int derivative, Eigen::VectorXcd* roots) const;
+     170             : 
+     171             :   // Finds all candidates for the minimum and maximum between t_start and t_end
+     172             :   // by evaluating the roots of the polynomial's derivative.
+     173             :   static bool selectMinMaxCandidatesFromRoots(double t_start, double t_end, const Eigen::VectorXcd& roots_derivative_of_derivative,
+     174             :                                               std::vector<double>* candidates);
+     175             : 
+     176             :   // Finds all candidates for the minimum and maximum between t_start and t_end
+     177             :   // by computing the roots of the derivative polynomial.
+     178             :   bool computeMinMaxCandidates(double t_start, double t_end, int derivative, std::vector<double>* candidates) const;
+     179             : 
+     180             :   // Evaluates the minimum and maximum of a polynomial between time t_start and
+     181             :   // t_end given the roots of the derivative.
+     182             :   // Returns the minimum and maximum as pair<t, value>.
+     183             :   bool selectMinMaxFromRoots(double t_start, double t_end, int derivative, const Eigen::VectorXcd& roots_derivative_of_derivative,
+     184             :                              std::pair<double, double>* minimum, std::pair<double, double>* maximum) const;
+     185             : 
+     186             :   // Computes the minimum and maximum of a polynomial between time t_start and
+     187             :   // t_end by computing the roots of the derivative polynomial.
+     188             :   // Returns the minimum and maximum as pair<t, value>.
+     189             :   bool computeMinMax(double t_start, double t_end, int derivative, std::pair<double, double>* minimum, std::pair<double, double>* maximum) const;
+     190             : 
+     191             :   // Selects the minimum and maximum of a polynomial among a candidate set.
+     192             :   // Returns the minimum and maximum as pair<t, value>.
+     193             :   bool selectMinMaxFromCandidates(const std::vector<double>& candidates, int derivative, std::pair<double, double>* minimum,
+     194             :                                   std::pair<double, double>* maximum) const;
+     195             : 
+     196             :   // Increase the number of coefficients of this polynomial up to the specified
+     197             :   // degree by appending zeros.
+     198             :   bool getPolynomialWithAppendedCoefficients(int new_N, Polynomial* new_polynomial) const;
+     199             : 
+     200             :   // Computes the base coefficients with the according powers of t, as
+     201             :   // e.g. needed for computation of (in)equality constraints.
+     202             :   // Output: coeffs = vector to write the coefficients to
+     203             :   // Input: polynomial derivative for which the coefficients have to
+     204             :   // be computed
+     205             :   // Input: t = time of evaluation
+     206             :   /* baseCoeffsWithTime() //{ */
+     207             : 
+     208      236760 :   static void baseCoeffsWithTime(int N, int derivative, double t, Eigen::VectorXd* coeffs) {
+     209      236760 :     CHECK_LT(derivative, N);
+     210      236760 :     CHECK_GE(derivative, 0);
+     211             : 
+     212      236760 :     coeffs->resize(N, 1);
+     213      236760 :     coeffs->setZero();
+     214             :     // first coefficient doesn't get multiplied
+     215      236760 :     (*coeffs)[derivative] = base_coefficients_(derivative, derivative);
+     216             : 
+     217      236760 :     if (std::abs(t) < std::numeric_limits<double>::epsilon())
+     218             :       return;
+     219             : 
+     220      118380 :     double t_power = t;
+     221             :     // now multiply increasing power of t towards the right
+     222      947040 :     for (int j = derivative + 1; j < N; j++) {
+     223      828660 :       (*coeffs)[j] = base_coefficients_(derivative, j) * t_power;
+     224      828660 :       t_power      = t_power * t;
+     225             :     }
+     226             :   }
+     227             : 
+     228             :   //}
+     229             : 
+     230             :   // Convenience method to compute the base coefficents with time
+     231             :   // static void baseCoeffsWithTime(const Eigen::MatrixBase<Derived> &
+     232             :   // coeffs, int derivative, double t)
+     233      236760 :   static Eigen::VectorXd baseCoeffsWithTime(int N, int derivative, double t) {
+     234      236760 :     Eigen::VectorXd c(N);
+     235      236760 :     baseCoeffsWithTime(N, derivative, t, &c);
+     236      236760 :     return c;
+     237             :   }
+     238             : 
+     239             :   // Discrete convolution of two vectors.
+     240             :   // convolve(d, k)[m] = sum(d[m - n] * k[n])
+     241             :   static Eigen::VectorXd convolve(const Eigen::VectorXd& data, const Eigen::VectorXd& kernel);
+     242             : 
+     243        4104 :   static inline int getConvolutionLength(int data_size, int kernel_size) {
+     244        4104 :     return data_size + kernel_size - 1;
+     245             :   }
+     246             : 
+     247             :   // Scales the polynomial in time with a scaling factor.
+     248             :   // To stretch out by a factor of 10, pass scaling_factor (b) = 1/10. To shrink
+     249             :   // by a factor of 10, pass scalign factor = 10.
+     250             :   // p_out = a12*b^12*t^12 + a11*b^11*t^11... etc.
+     251             :   void scalePolynomialInTime(double scaling_factor);
+     252             : 
+     253             :   // Offset this polynomial.
+     254             :   void offsetPolynomial(const double offset);
+     255             : 
+     256             : private:
+     257             :   int             N_;
+     258             :   Eigen::VectorXd coefficients_;
+     259             : };
+     260             : 
+     261             : // Static functions to compute base coefficients.
+     262             : 
+     263             : // Computes the base coefficients of the derivatives of the polynomial,
+     264             : // up to order N.
+     265             : Eigen::MatrixXd computeBaseCoefficients(int N);
+     266             : 
+     267             : }  // namespace eth_trajectory_generation
+     268             : 
+     269             : #endif  // ETH_TRAJECTORY_GENERATION_POLYNOMIAL_H_
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html new file mode 100644 index 0000000000..8941f11d19 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html @@ -0,0 +1,88 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8f24c71ffff3f844576638ad70e091a0d44c0e7f GIT binary patch literal 1546 zcmV+l2KD)gP)`Q)pk1L5CSsgX9#cgqg5Z^1jf6iYx zFMzJ|UyLpw0XuLNAbu9!zz_+41P5>c=keG9yd*V5LP8gK)d<5sK-7cL5m>+o=O@gY zOZpOz%>oDT3P#i(khb%i0HT3tNQ+?z9#ArJN(6R4pgo;8`uPG1FwC{H zNnD_v6(yK}l?-E-1RsE&;F*n3Vh(Sv##YC%ALmMs@p=Ou`ve97YZveGxV}o(2&5Gg ze@j#mVRgI>-Zsqoi4?*H4iqKQ;1gpW$%27%OMWf{;o~H-wzX=~f;wbA471`g_`=as zMQn0Hg=lP%5-HRpbB_D#ao|e(>28Ux_Qs>O2SesHaI?pfH)f2BfZ}FR+$gxVD#GyY zA(>KAH;zMuA$fNM+W98F`;IUV%@-0s^?lU0FOWV_q$9B$h%%)eZ;G|vdc^&?u<2t- z-oy3rApU&4UO%tj7p))q|Gx42`&4UtKIP-rkHEj4_1d1l%{_PBBkfSUL0a}T$7dC$ zqAdl@&ZkV{MUG|52*S|NZXp>W_(ToLIS1=JO35lktIhGu%56PH{gjR@*LmbR9DqqP z7f^zIkWf}~U4zjP#uLSL3YYJ%)Ld)~W-~-Daf`evwFl|>Kx#9>Nb2ME3!HFNL=N#Y zjf2sm-VRO3G$5e+DQ>``2JF{^yuM(W&-YT+aX}^vC_Pv}v|@1ly2AKL6FyW@)$dX+ z33inJF#v()ysRA6g0tzsZP<=qHAyuE)rKFw; zvsTaiLtBCO0${M0coi!^owigg$EbGx(-=Xz)kqye75SK^KZ&{}OzA10>_;!rs2t;V zZwgQ~!c8u?$0T5j9VUSyYw!&OJX7p~D^aSsT-G~iKzu`h-J*+VpFEe%JZ5aS1Tp-LWSs@Xa2*R8loNNu0q1p85PgfUS%RRaeG?w0k_34bPJ}K5LSz9024P zMG4_>&dIu2+zMeF>5Hi0uL*Tk81AriH(BMax)0uTQ0(x+F~U(OC|tlA6~v7i`Uy)VtW5^Oz2q2l0tl z_6)y%M1fKs9kp|kGE1q;)*bKc7I3Gswa@7Pazs+XX^7_Z)lLh+MpIi?1&uLiygp^q zN*TM9{c#TSHa%Zk-GvfKyvB^2zop00z2FEbdi4y~IauU@@^Bt0(XkFyPC;@zrsVFo9}okp z-s>XV8uywlT&WiolB=}Rhg+k2VM!syLC-r2yNuCRg{tn;{Q`ZT wWIk4`eTJ_7U_; + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_linear.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:192965.5 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimization<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const14
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html new file mode 100644 index 0000000000..e420507da3 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_linear.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:192965.5 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::PolynomialOptimization<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const14
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html new file mode 100644 index 0000000000..f7c6acfa52 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html new file mode 100644 index 0000000000..4c41af5ff3 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html @@ -0,0 +1,401 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_linear.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:192965.5 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..45a6c92eda95fd137b9c10f3ed72e56be7d7d1fa GIT binary patch literal 1756 zcmV<21|#{2P)16Bpgc=lV(U?bjRL8 zIK1=jkaQXiK1+B;bn(ODSrEQs-%|A4fG%N=bU@hBjtCo;1H!P5QI4Oa)bV_T+1K-S z!_+I{%YhhQWvU2vVM?>iyM`kmLLM;R8SFO!;pBQ!q1!%$iFhtSb1Ax8o{4U_KxU8C_;)Vg%DS<#*R{4N&uO- zZatQ@B@@cJzn>0u6a+vqrInOX1#3FXN;k+R!!bcsgFhtcqRbX<_E=B$Nao%;AXdk5 zWDaGWF(xph)f?Qjj4~B20SLLoI+9r8$0{xjo{G-mjXB40oLM=VK)*B|soRf?(b1RV zfl<_vdLEdY^npHSlKJrLVBIy$fuF0pn|qD+H@u73>&o- zMlO9D{@gQY74nJ-3iTuF4uAMVUW@6u?VO)T;CW#6e?&?k3m-P*4a2QY@h= zGYK>khOQlDl}giIwQDu(nbkit(c*?xPu+_CGEztq%1Vc%Ap4YwGbW=TjDzR55|@=H zii(zyO!X|8t;KcKA+amL+1hdH)EQ7)bTK19zGv`k9*yGGK7!E(~kd< zk8ZM7>xiD7L5d;r=na-NPAmWEoe zKUM^(PsSlImAxs#JHANWQbEue#c$j@_ovK-J>!SjnKS%%g|~Y?MnibHSxjjgaE}x} z){o^p+2>J;@3TqmX8ce!PzB%Iq(3 z)3SIiy2OL={#|nDxbAfvG`0;#ELyUWYZ@%VDqL=Mer&b0`on2dwaLZ(#}(gT8v_!p#7)`Q>hrp{yAw)S>DbUuA)Hy%k_$?pFMN$mZW=w!DZ zQIjE8+zt|Pz+#DnOQMf{gk*H|$0|`8(H{s^(Wb~`?ffy%m2jD}UdPr)Z>rnsD3b_* zG=vdFCpx|vM}7Va`H(r9V*A+gW2_sD|fLH7@?)|O({5df+H0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_nonlinear.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html new file mode 100644 index 0000000000..cf3633d619 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_nonlinear.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html new file mode 100644 index 0000000000..12ac5e7f12 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html new file mode 100644 index 0000000000..2391b8b710 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html @@ -0,0 +1,399 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_nonlinear.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:77100.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1247ecf14bfaf217dbdf10efdb3e578385816e8b GIT binary patch literal 1665 zcmV-{27dX8P)DV0Y5T4P#sm^?!IqC55$ z!r?nt55j4DP7iw*V(KzS)H@GWMV%z;IxzM* z?pvKGyC`Wb$&P1R49WZnk=p|IH+8%(yP}TSYwD`jg#Q4J z|E`WlbEWoLbUBpOKq!_jTaO z?#Bpv!fpBGMfoFeVuY!6hnFGRXPKEj09>^}s^@+;ChQ-r2{8>9FN!9NyfzwQ z{LZgYHcG`2Y%I7_8xW_h)-r+;Yec;FVNqHw8LJviJm;L-PY+h+QKao>bs=ly=a-ue7_PR?WcE)zZ&Fp7^h&0E6b6|FIQFcgun z=2#IH7HdKWeS=U39Fw=1anRiFLXwPhNU^A&93gmowElr^$7R*pUgCWV`-M6RD91i( z2|c!jO{r%t@(wGV`t_ELHo;mOZFwuiEW*}+!D9;+; zFMqdF|3Zihws@ZBx`ZX7aNTJi)ck6x{~l+8O9~p5wB<;|bi;A5cO9{5%~mPQibzwk zDQg1J*v&Ilo~`K8MnNb(W!kPMbsE|D3nKLk#VWBWXRxdk94(b#^*URtcJ16S7$=gx zcQ`0;kzuPJiFy|WKp`>F^y!oR zabEWSs~Xs!&v$dWll^mZ2Y76*3QsG0QYBX1jm(8xfLFs=<$CpDZ3DyO{KEGxX7$SM zU8|!9U(uI;+-=#{ujRJ)`~|C@9rfC13_1=&u@LUYYe9ofPwDh(N z2oY}9p3NS2_9iYCu&#-n7T#B6K-d+%eC2p`to_B~4HIiwf8V2mX$v2J+Z*m5e6mkr zm$Y7roH^c5$`BgA;$9e%$=IZ7y+BST+d%E3;vd!rvQWv0^1<3_ZWJ*rT-j4eIRd;{ zW&Z=-9Dn&Mz`X`pSt7y>XVw~uuMFlT61_9{kk0_OuV@ryUczZD>@lAQg~!CDppc!Q zD2qLb=eU6ACEAYTGI(au!&$o{lSOmdp?6#S_}WZiFfq%pV6fc>Wwl%wVyeq$u8+i=qE*UmEDva|t@YyNs z)=aj=pzGKmRr22_?l8G->KJUc$BmAF=dR1CkC0)Ny5xX5_(tb1h7*mg;Zjag00000 LNkvXXu0mjfgnJw( literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html new file mode 100644 index 0000000000..35d8188734 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - segment.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1313100.0 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Segment::Segment(int, int)14
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html new file mode 100644 index 0000000000..998262336d --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - segment.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1313100.0 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Segment::Segment(int, int)14
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html new file mode 100644 index 0000000000..a87cffca2c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html new file mode 100644 index 0000000000..52226d9dd3 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html @@ -0,0 +1,231 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - segment.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1313100.0 %
Date:2024-01-07 21:47:14Functions:11100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_SEGMENT_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_SEGMENT_H_
+      23             : 
+      24             : #include <Eigen/Core>
+      25             : #include <chrono>
+      26             : #include <map>
+      27             : #include <vector>
+      28             : 
+      29             : #include <eth_trajectory_generation/misc.h>
+      30             : #include <eth_trajectory_generation/extremum.h>
+      31             : #include <eth_trajectory_generation/motion_defines.h>
+      32             : #include <eth_trajectory_generation/polynomial.h>
+      33             : 
+      34             : namespace eth_trajectory_generation
+      35             : {
+      36             : 
+      37             : constexpr double kNumNSecPerSec = 1.0e9;
+      38             : constexpr double kNumSecPerNsec = 1.0e-9;
+      39             : 
+      40             : // Class holding the properties of parametric segment of a path.
+      41             : // Time of the segment and one polynomial for each dimension.
+      42             : //    X------------X---------------X
+      43             : //  vertex             segment
+      44         926 : class Segment {
+      45             : public:
+      46             :   typedef std::vector<Segment> Vector;
+      47             : 
+      48          14 :   Segment(int N, int D) : time_(0.0), N_(N), D_(D) {
+      49          14 :     polynomials_.resize(D_, Polynomial(N_));
+      50          14 :   }
+      51         912 :   Segment(const Segment& segment) = default;
+      52             : 
+      53             :   bool        operator==(const Segment& rhs) const;
+      54             :   inline bool operator!=(const Segment& rhs) const {
+      55             :     return !operator==(rhs);
+      56             :   }
+      57             : 
+      58        1624 :   int D() const {
+      59        1624 :     return D_;
+      60             :   }
+      61         484 :   int N() const {
+      62         484 :     return N_;
+      63             :   }
+      64       21040 :   double getTime() const {
+      65       19970 :     return time_;
+      66             :   }
+      67             :   uint64_t getTimeNSec() const {
+      68             :     return static_cast<uint64_t>(kNumNSecPerSec * time_);
+      69             :   }
+      70             : 
+      71       94020 :   void setTime(double time_sec) {
+      72       94020 :     time_ = time_sec;
+      73             :   }
+      74             :   void setTimeNSec(uint64_t time_ns) {
+      75             :     time_ = time_ns * kNumSecPerNsec;
+      76             :   }
+      77             : 
+      78             :   Polynomial& operator[](size_t idx);
+      79             : 
+      80             :   const Polynomial& operator[](size_t idx) const;
+      81             : 
+      82             :   const Polynomial::Vector& getPolynomialsRef() const {
+      83             :     return polynomials_;
+      84             :   }
+      85             : 
+      86             :   Eigen::VectorXd evaluate(double t, int derivative_order = derivative_order::POSITION) const;
+      87             : 
+      88             :   // Computes the candidates for the minimum and maximum magnitude of a single
+      89             :   // segment in the specified derivative. In the 1D case, it simply returns the
+      90             :   // roots of the derivative of the segment-polynomial. For higher dimensions,
+      91             :   // e.g. 3D, we need to find the extrema of \sqrt{x(t)^2 + y(t)^2 + z(t)^2}
+      92             :   // where x, y, z are polynomials describing the position or the derivative,
+      93             :   // specified by Derivative. Taking the derivative yields
+      94             :   // 2 x \dot{x} + 2 y \dot{y} + 2 z \dot{z}, which needs to be zero at the
+      95             :   // extrema. The multiplication of two polynomials is a convolution of their
+      96             :   // coefficients. Re-ordering by their powers and addition yields a polynomial,
+      97             :   // for which we can compute the roots.
+      98             :   // Input: derivative = Derivative of position, in which to find the maxima.
+      99             :   // Input: t_start = Only maxima >= t_start are returned. Usually set to 0.
+     100             :   // Input: t_stop = Only maxima <= t_stop are returned. Usually set to segment
+     101             :   // time.
+     102             :   // Input: dimensions = Vector containing the dimensions that are evaluated.
+     103             :   // Usually [0, 1, 2] for position, [3] for yaw.
+     104             :   // Output: candidates = Vector containing the candidate extrema times.
+     105             :   // Returns whether the computation succeeded -- false means no candidates
+     106             :   // were found by Jenkins-Traub.
+     107             :   bool computeMinMaxMagnitudeCandidateTimes(int derivative, double t_start, double t_end, const std::vector<int>& dimensions,
+     108             :                                             std::vector<double>* candidate_times) const;
+     109             : 
+     110             :   // Convenience function. Additionally evaluates the candidate times.
+     111             :   bool computeMinMaxMagnitudeCandidates(int derivative, double t_start, double t_end, const std::vector<int>& dimensions,
+     112             :                                         std::vector<Extremum>* candidates) const;
+     113             : 
+     114             :   // Convenience function. Evaluates the magnitudes between t_start and t_end
+     115             :   // for a set of candidates for given dimensions.
+     116             :   bool selectMinMaxMagnitudeFromCandidates(int derivative, double t_start, double t_end, const std::vector<int>& dimensions,
+     117             :                                            const std::vector<Extremum>& candidates, Extremum* minimum, Extremum* maximum) const;
+     118             : 
+     119             :   // Split a segment to get a segment with the specified dimension.
+     120             :   bool getSegmentWithSingleDimension(int dimension, Segment* new_segment) const;
+     121             :   // Compose this segment and another segment to a new segment.
+     122             :   bool getSegmentWithAppendedDimension(const Segment& segment_to_append, Segment* new_segment) const;
+     123             : 
+     124             :   // Offset this segment by vector A_r_B.
+     125             :   bool offsetSegment(const Eigen::VectorXd& A_r_B);
+     126             : 
+     127             : protected:
+     128             :   Polynomial::Vector polynomials_;
+     129             :   double             time_;
+     130             : 
+     131             : private:
+     132             :   int N_;  // Number of coefficients.
+     133             :   int D_;  // Number of dimensions.
+     134             : };
+     135             : 
+     136             : // Prints the properties of the segment.
+     137             : // Polynomial coefficients are printed with increasing powers,
+     138             : // i.e. c_0 + c_1*t ... c_{N-1} * t^{N-1}
+     139             : void printSegment(std::ostream& stream, const Segment& s, int derivative);
+     140             : 
+     141             : std::ostream& operator<<(std::ostream& stream, const Segment& s);
+     142             : 
+     143             : std::ostream& operator<<(std::ostream& stream, const std::vector<Segment>& segments);
+     144             : 
+     145             : }  // namespace eth_trajectory_generation
+     146             : 
+     147             : #endif  // ETH_TRAJECTORY_GENERATION_SEGMENT_H_
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html new file mode 100644 index 0000000000..29e01f7518 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..652bb2770fe633b7bfd66724304f0d13b32f0589 GIT binary patch literal 899 zcmV-}1AP36P)qCQt0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp=>6|DN>(5s*3JxVpIX0Jei9%qX_*`V=^Su=t}k8}$k+Lo zQV3*lfUAIX4^A*9mY*;H4sel;9pEFcF|iQ7psU6i`5pTPJRez68b7q{kF))I5fGxHM{M@-z2fHWg>*=Vf*&%dwcMp7Lj?MCA zapzN?R(W15E89(@&WJ>2y3td;=soD8&T$<3asKx8W-J%#4wQjaVUN{MjrF+3er(6K z?|gooSI1VVUI}?9CXc|zb;^|s=wxk~sGh{hanX|lI3JmRR87ecq+qD}ws7WE`?)zx1smZc9?WXLkQBrYmrXZ9nI0;r zvHOqcF`sF3)Tf8IkOKSL70K!vzp!c09lR47%_z?$7E-gVwNOj5vBY2E(ae(-RMUK7 zLaZgyB&+*+w|dhiTXqk!pGPwQxe-9ywj*x=R3a8!+Gbda*Y_c`KkcW<_6%gWE9 zdFKdoK=h3!SdWy0Fv48NgE=u%*j z6`(;B&6H-ES%Vp((*d|-%E#{0GDON0gU*?vt;NdmfsKuYofW`6qUzq*=!#2onPUy$ zUKP}@Mw~$r;i^VD{n#8Rp2kqY*cp%+jXiLsU|AB_s9w8JX+Bz2YtvIT$)VT!W<#$Y zI>Vh4*K{Y#y=q-I(x@ixG*8oB>m32<k literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html new file mode 100644 index 0000000000..e96393f56b --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - timing.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0320.0 %
Date:2024-01-07 21:47:14Functions:020.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::timing::Accumulator<double, double, 50>::Add(double)0
eth_trajectory_generation::timing::Accumulator<double, double, 50>::LazyVariance() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html new file mode 100644 index 0000000000..cc60bf9907 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - timing.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0320.0 %
Date:2024-01-07 21:47:14Functions:020.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::timing::Accumulator<double, double, 50>::Add(double)0
eth_trajectory_generation::timing::Accumulator<double, double, 50>::LazyVariance() const0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html new file mode 100644 index 0000000000..2699d2a11a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html new file mode 100644 index 0000000000..e98a8e82f1 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html @@ -0,0 +1,322 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - timing.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0320.0 %
Date:2024-01-07 21:47:14Functions:020.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..99b3a26b1866150aa650537725f0658ba5a7e50d GIT binary patch literal 1036 zcmV+n1oQieP)qV$0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vplDp8i`|TLK*x5@=RVUdZm7Oy-9hA#gU@oCH7@!S!|j!}|^L z`|zJg1u|H{J%I5uxWGuvpHKk~aGH)C;FQ%!%!J>>ca1Ufg)AK^A7Kq7ygw1$RMAs9 zHiQarhGX;|vbV!GfXM??iB5rGJVwj#kreEZ^peAb$wlHGl2?u*|Fm!5s_1>8R+1*;Uj6Yva-hoVnQ zz0C2b{1`lUJek*`dkSXYF;Iw=}s+cIa&uZi}Sf)92B zQ`L>liII_F7;#*GO0i_i-&se~>eqBU@5=J%n98+>e&^Ve2o8N&#`=(u^RNAoj(;K6 ze?doXtNom;THJL;kAR%HrZHY?nBmpv435v583FIEd0ci>?eP56%`_Nuvf<~MyXT$m zXU);y70%D7Sf0k%9G4fD<4!L~+~j)e1sXw5mu6c@^j#q_V3z|ugO7vP9Ho)VV(mUC zN_r_PyY609gfLn$(fX1;f3wh_--$0s!-<6n+O{ z)Y9k=vc<`ZCRurfGLcF!moNUYwRB?^#RFR@YHB8BDi>$@hrqMoBu8_8d}qo9kH4pk z0hcY499=v)B+EpJ)Ps`^ACANlA4-n)%XXIe3(wE5M6Kp0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - trajectory.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:244158.5 %
Date:2024-01-07 21:47:14Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Trajectory::clear()0
eth_trajectory_generation::Trajectory::addSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)28
eth_trajectory_generation::Trajectory::setSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)28
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html new file mode 100644 index 0000000000..7cb3c853ba --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - trajectory.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:244158.5 %
Date:2024-01-07 21:47:14Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Trajectory::addSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)28
eth_trajectory_generation::Trajectory::setSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)28
eth_trajectory_generation::Trajectory::clear()0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html new file mode 100644 index 0000000000..c189d69eb8 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html new file mode 100644 index 0000000000..6833536928 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html @@ -0,0 +1,261 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - trajectory.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:244158.5 %
Date:2024-01-07 21:47:14Functions:2366.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_TRAJECTORY_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_TRAJECTORY_H_
+      23             : 
+      24             : #include <eth_trajectory_generation/extremum.h>
+      25             : #include <eth_trajectory_generation/segment.h>
+      26             : #include <eth_trajectory_generation/vertex.h>
+      27             : 
+      28             : namespace eth_trajectory_generation
+      29             : {
+      30             : 
+      31             : // Holder class for trajectories of D dimensions, of K segments, and
+      32             : // polynomial order N-1. (N=12 -> 11th order polynomial, with 12 coefficients).
+      33           0 : class Trajectory {
+      34             : public:
+      35          28 :   Trajectory() : D_(0), N_(0), max_time_(0.0) {
+      36             :   }
+      37          28 :   ~Trajectory() {
+      38          14 :   }
+      39             : 
+      40             :   bool        operator==(const Trajectory& rhs) const;
+      41             :   inline bool operator!=(const Trajectory& rhs) const {
+      42             :     return !operator==(rhs);
+      43             :   }
+      44             : 
+      45        1942 :   int D() const {
+      46        1942 :     return D_;
+      47             :   }
+      48           0 :   int N() const {
+      49           0 :     return N_;
+      50             :   }
+      51           0 :   int K() const {
+      52           0 :     return segments_.size();
+      53             :   }
+      54             : 
+      55             :   bool empty() const {
+      56             :     return segments_.empty();
+      57             :   }
+      58           0 :   void clear() {
+      59           0 :     segments_.clear();
+      60           0 :     D_        = 0;
+      61           0 :     N_        = 0;
+      62           0 :     max_time_ = 0.0;
+      63           0 :   }
+      64             : 
+      65          28 :   void setSegments(const Segment::Vector& segments) {
+      66          28 :     CHECK(!segments.empty());
+      67             :     // Reset states.
+      68          28 :     D_        = segments.front().D();
+      69          28 :     N_        = segments.front().N();
+      70          28 :     max_time_ = 0.0;
+      71          28 :     segments_.clear();
+      72             : 
+      73          28 :     addSegments(segments);
+      74          28 :   }
+      75             : 
+      76          28 :   void addSegments(const Segment::Vector& segments) {
+      77         484 :     for (const Segment& segment : segments) {
+      78         456 :       CHECK_EQ(segment.D(), D_);
+      79         456 :       CHECK_EQ(segment.N(), N_);
+      80         456 :       max_time_ += segment.getTime();
+      81             :     }
+      82          28 :     segments_.insert(segments_.end(), segments.begin(), segments.end());
+      83          28 :   }
+      84             : 
+      85           0 :   void getSegments(Segment::Vector* segments) const {
+      86           0 :     CHECK_NOTNULL(segments);
+      87           0 :     *segments = segments_;
+      88           0 :   }
+      89             : 
+      90           0 :   const Segment::Vector& segments() const {
+      91           0 :     return segments_;
+      92             :   }
+      93             : 
+      94          28 :   double getMinTime() const {
+      95          28 :     return 0.0;
+      96             :   }
+      97          28 :   double getMaxTime() const {
+      98          28 :     return max_time_;
+      99             :   }
+     100             :   std::vector<double> getSegmentTimes() const;
+     101             : 
+     102             :   // Functions to create new trajectories by splitting (getting a NEW trajectory
+     103             :   // with a single dimension) or compositing (create a new trajectory with
+     104             :   // another trajectory appended).
+     105             :   Trajectory getTrajectoryWithSingleDimension(int dimension) const;
+     106             :   bool       getTrajectoryWithAppendedDimension(const Trajectory& trajectory_to_append, Trajectory* new_trajectory) const;
+     107             : 
+     108             :   // Add trajectories with same dimensions and coefficients to this trajectory.
+     109             :   bool addTrajectories(const std::vector<Trajectory>& trajectories, Trajectory* merged) const;
+     110             : 
+     111             :   // Offset this trajectory by vector A_r_B.
+     112             :   bool offsetTrajectory(const Eigen::VectorXd& A_r_B);
+     113             : 
+     114             :   // Evaluate the vertex constraint at time t.
+     115             :   Vertex getVertexAtTime(double t, int max_derivative_order) const;
+     116             :   // Evaluate the vertex constraint at start time.
+     117             :   Vertex getStartVertex(int max_derivative_order) const;
+     118             :   // Evaluate the vertex constraint at goal time.
+     119             :   Vertex getGoalVertex(int max_derivative_order) const;
+     120             :   // Evaluate all underlying vertices.
+     121             :   bool getVertices(int max_derivative_order_pos, int max_derivative_order_yaw, Vertex::Vector* pos_vertices, Vertex::Vector* yaw_vertices) const;
+     122             :   bool getVertices(int max_derivative_order, Vertex::Vector* vertices) const;
+     123             : 
+     124             :   // Evaluation functions.
+     125             :   // Evaluate at a single time, and a single derivative. Return type of
+     126             :   // dimension D.
+     127             :   Eigen::VectorXd evaluate(double t, int derivative_order = derivative_order::POSITION) const;
+     128             : 
+     129             :   // Evaluates the trajectory in a specified range and derivative.
+     130             :   // Outputs are a vector of the sampled values (size of VectorXd is D) by
+     131             :   // time and optionally the actual sampling times.
+     132             :   void evaluateRange(double t_start, double t_end, double dt, int derivative_order, std::vector<Eigen::VectorXd>* result,
+     133             :                      std::vector<double>* sampling_times = nullptr) const;
+     134             : 
+     135             :   // Compute the analytic minimum and maximum of magnitude for a given
+     136             :   // derivative and dimensions, e.g., [0, 1, 2] for position or [3] for yaw.
+     137             :   // Returns false in case of extremum calculation failure.
+     138             :   bool computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum, int seg) const;
+     139             : 
+     140             :   // Compute the analytic minimum and maximum of magnitude for a given
+     141             :   // derivative and dimensions, e.g., [0, 1, 2] for position or [3] for yaw.
+     142             :   // Returns false in case of extremum calculation failure.
+     143             :   bool computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum) const;
+     144             : 
+     145             :   // Compute max velocity and max acceleration. Shorthand for the method above.
+     146             :   bool computeMaxDerivativesHorizontal(double* v_max, double* a_max, double* j_max, int seg) const;
+     147             :   bool computeMaxDerivativesHorizontal(double* v_max, double* a_max, double* j_max) const;
+     148             : 
+     149             :   bool computeMaxDerivativesVertical(double* v_max, double* a_max, double* j_max, int seg) const;
+     150             :   bool computeMaxDerivativesVertical(double* v_max, double* a_max, double* j_max) const;
+     151             : 
+     152             :   bool computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max, int seg) const;
+     153             :   bool computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max) const;
+     154             : 
+     155             :   // This method SCALES the segment times evenly.
+     156             :   bool scaleSegmentTimes(double scaling);
+     157             : 
+     158             :   // This method SCALES the segment times evenly to ensure that the trajectory
+     159             :   // is feasible given the provided v_max and a_max. Does not change the shape
+     160             :   // of the trajectory, and only *increases* segment times.
+     161             :   bool scaleSegmentTimesToMeetConstraints(const double v_max_translation_horizontal, const double v_max_translation_vertical,
+     162             :                                           const double a_max_translation_horizontal, const double a_max_translation_vertical,
+     163             :                                           const double j_max_translation_horizontal, const double j_max_translation_vertical, const double v_max_heading,
+     164             :                                           const double a_max_heading, const double j_max_heading);
+     165             : 
+     166             : private:
+     167             :   int    D_;         // Number of dimensions.
+     168             :   int    N_;         // Number of coefficients.
+     169             :   double max_time_;  // Time at the end of the trajectory.
+     170             : 
+     171             :   // K is number of segments...
+     172             :   Segment::Vector segments_;
+     173             : };
+     174             : 
+     175             : }  // namespace eth_trajectory_generation
+     176             : 
+     177             : #endif  // ETH_TRAJECTORY_GENERATION_TRAJECTORY_H_
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html new file mode 100644 index 0000000000..141f034ccd --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html @@ -0,0 +1,65 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6f57a558d660c45ceb4654d3c05c54190f98ad5a GIT binary patch literal 979 zcmV;^11$WBP)`E3EiZ-#u$DfNr%EmSONpaUsyL& zv`fc^&;WWkChsA6`~Ctb9teeK2@K&eS%y_okVE8>!-V1@c2D3dMKRU{!;FHGjRH zuSoySF&Sx5o`UvZmb- zuKB0RN3ps1l7Js~_^Hb1O;7r$YOpRTlZq<^U|fB^PykNuka%?LaZ80+wb?PHhB z9&hiwu}>rJ>&-l(hqcisTDr2_b{Tz{i|8Ph&ceX0H8*s~efJGSJ~2x>eQ)6)t#{@K zS;@ZW&OwErV+LLy){z+1r^^k>R_S52`7&E56x1qUp@GxksNL9$U?Hz)vmMeJ(Q58N zmYSkV*-&`_M;zmO$PqQ`8f)ulABRDyWNFuMBxKJ$!l%eJsBSyJ;!&%`n3mRyG6v*S zERN#5Ld|huQc?v)2OCwS+{Q>%;B&e#E`dPC$UG5ccofSL19JfL9I*AVj);KxA~OVS zOob02@PN>{%w)u+28y|qLS7W?7b#xI)3p#}%BDD`QAQq`wX?A*VHM-z$)7Pg2{5R8 zXp6m&3^e(~>Hx0mXmM;9_C#C2w?r#W%-8|+L`JPn3A0t~o4xnVSy4wcHA1=V*>bkZ zdf=SlD+rLYRsNa4zL_~Zds3*Q_jiGq`?xhXE(iSbL0SX0xcwKjZ{-mZC>jZ z6dYluTox^fIFj{NYi<$(X33%J8wW)T%#5sX{{i6wS7^U7Wa$6^002ovPDHLkV1j0_ B)ky#V literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html new file mode 100644 index 0000000000..9e38fc453d --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - vertex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6875.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html new file mode 100644 index 0000000000..4bf9a0480b --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - vertex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6875.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + +

Function Name Sort by function nameHit count Sort by hit count
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html new file mode 100644 index 0000000000..a88ee3993a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html new file mode 100644 index 0000000000..6f04ed8d3e --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - vertex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6875.0 %
Date:2024-01-07 21:47:14Functions:00-
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_VERTEX_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_VERTEX_H_
+      23             : 
+      24             : #include <Eigen/Core>
+      25             : #include <chrono>
+      26             : #include <map>
+      27             : #include <vector>
+      28             : #include <eth_trajectory_generation/misc.h>
+      29             : #include <eth_trajectory_generation/motion_defines.h>
+      30             : #include <eth_trajectory_generation/polynomial.h>
+      31             : 
+      32             : namespace eth_trajectory_generation
+      33             : {
+      34             : 
+      35             : // A vertex describes the properties of a support point of a path.
+      36             : // A vertex has a set of constraints, the derivative of position a value, that
+      37             : // have to be matched during optimization procedures. In case of a
+      38             : // multi-dimensional vertex (mostly 3D), the constraint for a derivative
+      39             : // exists in all dimensions, but can have different values in each dimension.
+      40             : //     X------------X---------------X
+      41             : //   vertex             segment
+      42        2100 : class Vertex {
+      43             : public:
+      44             :   typedef std::vector<Vertex>             Vector;
+      45             :   typedef Eigen::VectorXd                 ConstraintValue;
+      46             :   typedef std::pair<int, ConstraintValue> Constraint;
+      47             :   typedef std::map<int, ConstraintValue>  Constraints;
+      48             : 
+      49             :   // Constructs an empty vertex and sets time_to_next and
+      50             :   // derivative_to_optimize to zero.
+      51         489 :   Vertex(size_t dimension) : D_(dimension) {
+      52             :   }
+      53             : 
+      54           0 :   int D() const {
+      55           0 :     return D_;
+      56             :   }
+      57             : 
+      58             :   // Adds a constraint for the specified derivative order with the given
+      59             :   // value. If this is a multi-dimensional vertex, all dimensions are
+      60             :   // set to the same value.
+      61             :   inline void addConstraint(int derivative_order, double value) {
+      62             :     constraints_[derivative_order] = ConstraintValue::Constant(D_, value);
+      63             :   }
+      64             : 
+      65             :   // Adds a constraint for the derivative specified in type with the given
+      66             :   // values in the constraint vector. The dimension has to match the derivative.
+      67             :   void addConstraint(int type, const Eigen::VectorXd& constraint);
+      68             : 
+      69             :   // Removes a constraint for the derivative specified in type. Returns false if
+      70             :   // constraint was not set.
+      71             :   bool removeConstraint(int type);
+      72             : 
+      73             :   // Sets a constraint for position and sets all derivatives up to
+      74             :   // (including) up_to_derivative to zero. Convenience method for
+      75             :   // beginning / end vertices. up_to_derivative should be set to
+      76             :   // getHighestDerivativeFromN(N), where N is the order of your polynomial.
+      77             :   void makeStartOrEnd(const Eigen::VectorXd& constraint, int up_to_derivative);
+      78             : 
+      79             :   void makeStartOrEnd(double value, int up_to_derivative) {
+      80             :     makeStartOrEnd(Eigen::VectorXd::Constant(D_, value), up_to_derivative);
+      81             :   }
+      82             : 
+      83             :   // Returns whether the vertex has a constraint for the specified derivative
+      84             :   // order.
+      85             :   bool hasConstraint(int derivative_order) const;
+      86             : 
+      87             :   // Passes the value of the constraint for derivative order to *value,
+      88             :   // and returns whether the constraint is set.
+      89             :   bool getConstraint(int derivative_order, Eigen::VectorXd* constraint) const;
+      90             : 
+      91             :   // Returns a const iterator to the first constraint.
+      92         242 :   typename Constraints::const_iterator cBegin() const {
+      93         242 :     return constraints_.begin();
+      94             :   }
+      95             : 
+      96             :   // Returns a const iterator to the end of the constraints,
+      97             :   // i.e. one after the last element.
+      98         554 :   typename Constraints::const_iterator cEnd() const {
+      99         554 :     return constraints_.end();
+     100             :   }
+     101             : 
+     102             :   // Returns the number of constraints.
+     103             :   size_t getNumberOfConstraints() const {
+     104             :     return constraints_.size();
+     105             :   }
+     106             : 
+     107             :   // Checks if both lhs and rhs are equal up to tol in case of double values.
+     108             :   bool isEqualTol(const Vertex& rhs, double tol) const;
+     109             : 
+     110             :   // Get subdimension vertex.
+     111             :   bool getSubdimension(const std::vector<size_t>& subdimensions, int max_derivative_order, Vertex* subvertex) const;
+     112             : 
+     113             : private:
+     114             :   int         D_;
+     115             :   Constraints constraints_;
+     116             : };
+     117             : 
+     118             : std::ostream& operator<<(std::ostream& stream, const Vertex& v);
+     119             : 
+     120             : std::ostream& operator<<(std::ostream& stream, const std::vector<Vertex>& vertices);
+     121             : 
+     122             : // Makes a rough estimate based on v_max and a_max about the time
+     123             : // required to get from one vertex to the next. Uses the current preferred
+     124             : // method.
+     125             : std::vector<double> estimateSegmentTimes(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     126             :                                          const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal, const double j_max_vertical,
+     127             :                                          const double heading_speed_max, const double heading_acc_max);
+     128             : 
+     129             : // Calculate the velocity assuming instantaneous constant acceleration a_max
+     130             : // and straight line rest-to-rest trajectories.
+     131             : // The time_factor \in [1..Inf] increases the allocated time making the segments
+     132             : // slower and thus feasibility more likely. This method does not take into
+     133             : // account the start and goal velocity and acceleration.
+     134             : std::vector<double> estimateSegmentTimesVelocityRamp(const Vertex::Vector& vertices, double v_max, double a_max, double time_factor = 1.0);
+     135             : 
+     136             : std::vector<double> estimateSegmentTimesEuclidean(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     137             :                                                   const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     138             :                                                   const double j_max_vertical, const double heading_speed_max, const double heading_acc_max);
+     139             : 
+     140             : std::vector<double> estimateSegmentTimesBaca(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     141             :                                              const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     142             :                                              const double j_max_vertical, const double heading_speed_max, const double heading_acc_max);
+     143             : 
+     144             : double computeTimeVelocityRamp(const Eigen::VectorXd& start, const Eigen::VectorXd& goal, double v_max, double a_max);
+     145             : 
+     146             : inline int getHighestDerivativeFromN(int N) {
+     147             :   return N / 2 - 1;
+     148             : }
+     149             : 
+     150             : // Creates random vertices for position within minimum_position and
+     151             : // maximum_position.
+     152             : // Vertices at the beginning and end have only fixed constraints with their
+     153             : // derivative set to zero, while  all vertices in between have position as fixed
+     154             : // constraint and the derivatives are left free.
+     155             : // Input: maximum_derivative = The maximum derivative to be set to zero for
+     156             : // beginning and end.
+     157             : // Input: n_segments = Number of segments of the resulting trajectory. Number
+     158             : // of vertices is n_segments + 1.
+     159             : // Input: minimum_position = Minimum position of the space to sample.
+     160             : // Input: maximum_position = Maximum position of the space to sample.
+     161             : // Input: seed = Initial seed for random number generation.
+     162             : // Output: return = Vector containing n_segments + 1 vertices.
+     163             : Vertex::Vector createRandomVertices(int maximum_derivative, size_t n_segments, const Eigen::VectorXd& minimum_position, const Eigen::VectorXd& maximum_position,
+     164             :                                     size_t seed = 0);
+     165             : 
+     166             : Vertex::Vector createSquareVertices(int maximum_derivative, const Eigen::Vector3d& center, double side_length, int rounds);
+     167             : 
+     168             : // Conveninence function to create 1D vertices.
+     169             : Vertex::Vector createRandomVertices1D(int maximum_derivative, size_t n_segments, double minimum_position, double maximum_position, size_t seed = 0);
+     170             : }  // namespace eth_trajectory_generation
+     171             : 
+     172             : #endif  // ETH_TRAJECTORY_GENERATION_VERTEX_H_
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html new file mode 100644 index 0000000000..81b47fa20f --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html @@ -0,0 +1,63 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ce4278780839271ff12ef6b741c85016395ff57e GIT binary patch literal 1058 zcmV+-1l{|IP)i(-d3HM7b1Z>6DG;+NEU+9*%$(2%muk?PEACw!= z{qdV~2qbWTYk+kxy}`&tKO+DRaFL81;F8wJM1*eY-ANgKfw(7UBY0p&*RsJZima~W9aq$YPgw$lDYjJBhyfSBw{70=#xWg(<$|NVq38Df zvG4o+cD3HX{c(vZK%a&q84M8SGqpG5^fxW;*^mv05LS#)Y)WSNTXtE0*$rQ+F>?oN{71)f3Qj|?<%SW~dkcKv9Ie6e^ z&bGG}f;B)P@rOujBS_M%l^AnLw$bj$UHOM`X>;d<1Y7CoA1(}&= zy7!p1j_D&JZGChJXq)CV=V>&R{A(4hL_=NX_%1AO46#w-%U@H}-35xt)(JD~&=nR) zl?2+QbqYxAI-TaGlxg)5O}Kf*$3pykVKO*o+5r0$yXMi6N2QZ{KAljEW=8` z>T6Blg(IaScV1I$X@*U%Y?O5+?)N5F6S8os!vbdvU}rZTfnf}o*e^PS*1aw3fie$5 z^>YF8REao8nkr+8qUqYo+SS&+GGJ;Mt#0&QPPvjh4exu>$tY*uJnQ-~d + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44899445.1 %
Date:2024-01-07 21:47:14Functions:3110529.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
motion_defines.cpp +
0.0%
+ 		0.0 %0 / 240.0 %0 / 4
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
<unnamed>42.0 %29 / 6928.6 %2 / 7
vertex.cpp +
50.8%50.8%
+ 		50.8 %133 / 26235.3 %6 / 17
<unnamed>50.8 %133 / 26235.3 %6 / 17
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
<unnamed>40.9 %52 / 12746.2 %6 / 13
trajectory.cpp +
61.1%61.1%
+ 		61.1 %182 / 29847.8 %11 / 23
<unnamed>61.1 %182 / 29847.8 %11 / 23
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
<unnamed>50.5 %52 / 10354.5 %6 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html new file mode 100644 index 0000000000..2cd255de97 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44899445.1 %
Date:2024-01-07 21:47:14Functions:3110529.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
motion_defines.cpp +
0.0%
+ 		0.0 %0 / 240.0 %0 / 4
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
<unnamed>40.9 %52 / 12746.2 %6 / 13
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
<unnamed>42.0 %29 / 6928.6 %2 / 7
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
<unnamed>50.5 %52 / 10354.5 %6 / 11
vertex.cpp +
50.8%50.8%
+ 		50.8 %133 / 26235.3 %6 / 17
<unnamed>50.8 %133 / 26235.3 %6 / 17
trajectory.cpp +
61.1%61.1%
+ 		61.1 %182 / 29847.8 %11 / 23
<unnamed>61.1 %182 / 29847.8 %11 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html new file mode 100644 index 0000000000..df2c75c1c7 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44899445.1 %
Date:2024-01-07 21:47:14Functions:3110529.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
motion_defines.cpp +
0.0%
+ 		0.0 %0 / 240.0 %0 / 4
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
<unnamed>50.5 %52 / 10354.5 %6 / 11
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
<unnamed>40.9 %52 / 12746.2 %6 / 13
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
trajectory.cpp +
61.1%61.1%
+ 		61.1 %182 / 29847.8 %11 / 23
<unnamed>61.1 %182 / 29847.8 %11 / 23
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
<unnamed>42.0 %29 / 6928.6 %2 / 7
vertex.cpp +
50.8%50.8%
+ 		50.8 %133 / 26235.3 %6 / 17
<unnamed>50.8 %133 / 26235.3 %6 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html new file mode 100644 index 0000000000..de27091ce9 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44899445.1 %
Date:2024-01-07 21:47:14Functions:3110529.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
motion_defines.cpp +
0.0%
+ 		0.0 %0 / 240.0 %0 / 4
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
vertex.cpp +
50.8%50.8%
+ 		50.8 %133 / 26235.3 %6 / 17
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
trajectory.cpp +
61.1%61.1%
+ 		61.1 %182 / 29847.8 %11 / 23
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html new file mode 100644 index 0000000000..df19c36187 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44899445.1 %
Date:2024-01-07 21:47:14Functions:3110529.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
motion_defines.cpp +
0.0%
+ 		0.0 %0 / 240.0 %0 / 4
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
vertex.cpp +
50.8%50.8%
+ 		50.8 %133 / 26235.3 %6 / 17
trajectory.cpp +
61.1%61.1%
+ 		61.1 %182 / 29847.8 %11 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html new file mode 100644 index 0000000000..41723db47e --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44899445.1 %
Date:2024-01-07 21:47:14Functions:3110529.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
motion_defines.cpp +
0.0%
+ 		0.0 %0 / 240.0 %0 / 4
polynomial.cpp +
50.5%50.5%
+ 		50.5 %52 / 10354.5 %6 / 11
segment.cpp +
40.9%40.9%
+ 		40.9 %52 / 12746.2 %6 / 13
timing.cpp +
0.0%
+ 		0.0 %0 / 1110.0 %0 / 30
trajectory.cpp +
61.1%61.1%
+ 		61.1 %182 / 29847.8 %11 / 23
trajectory_sampling.cpp +
42.0%42.0%
+ 		42.0 %29 / 6928.6 %2 / 7
vertex.cpp +
50.8%50.8%
+ 		50.8 %133 / 26235.3 %6 / 17
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html new file mode 100644 index 0000000000..1e11d2f043 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - motion_defines.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0240.0 %
Date:2024-01-07 21:47:14Functions:040.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

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

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

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/motion_defines.h>
+      22             : 
+      23             : namespace eth_trajectory_generation
+      24             : {
+      25             : 
+      26             : /* positionDerivativeToString //{ */
+      27             : 
+      28           0 : std::string positionDerivativeToString(int derivative) {
+      29           0 :   if (derivative >= derivative_order::POSITION && derivative <= derivative_order::SNAP) {
+      30           0 :     static constexpr const char* text[] = {"position", "velocity", "acceleration", "jerk", "snap"};
+      31           0 :     return std::string(text[derivative]);
+      32             :   } else {
+      33           0 :     return std::string("invalid");
+      34             :   }
+      35             : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* positionDerivativeToInt() //{ */
+      40             : 
+      41           0 : int positionDerivativeToInt(const std::string& string) {
+      42           0 :   using namespace derivative_order;
+      43           0 :   if (string == "position") {
+      44             :     return POSITION;
+      45           0 :   } else if (string == "velocity") {
+      46             :     return VELOCITY;
+      47           0 :   } else if (string == "acceleration") {
+      48             :     return ACCELERATION;
+      49           0 :   } else if (string == "jerk") {
+      50             :     return JERK;
+      51           0 :   } else if (string == "snap") {
+      52             :     return SNAP;
+      53             :   } else {
+      54           0 :     return INVALID;
+      55             :   }
+      56             : }
+      57             : 
+      58             : //}
+      59             : 
+      60             : /* orintationDerivativeToString() //{ */
+      61             : 
+      62           0 : std::string orintationDerivativeToString(int derivative) {
+      63           0 :   if (derivative >= derivative_order::ORIENTATION && derivative <= derivative_order::ANGULAR_ACCELERATION) {
+      64           0 :     static constexpr const char* text[] = {"orientation", "angular_velocity", "angular_acceleration"};
+      65           0 :     return std::string(text[derivative]);
+      66             :   } else {
+      67           0 :     return std::string("invalid");
+      68             :   }
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* orientationDerivativeToInt() //{ */
+      74             : 
+      75           0 : int orientationDerivativeToInt(const std::string& string) {
+      76           0 :   using namespace derivative_order;
+      77           0 :   if (string == "orientation") {
+      78             :     return ORIENTATION;
+      79           0 :   } else if (string == "angular_velocity") {
+      80             :     return ANGULAR_VELOCITY;
+      81           0 :   } else if (string == "angular_acceleration") {
+      82             :     return ANGULAR_ACCELERATION;
+      83             :   } else {
+      84           0 :     return INVALID;
+      85             :   }
+      86             : }
+      87             : 
+      88             : //}
+      89             : 
+      90             : }  // namespace eth_trajectory_generation
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html new file mode 100644 index 0000000000..3819ee7329 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html @@ -0,0 +1,43 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..adc83232603d9fb7f4a5b693f8d27b6804e6e6b4 GIT binary patch literal 468 zcmV;_0W1EAP)KSM3$UAPCeAU?;l)-G7zF2!2AMt^Lzik~THoI|NReICPWdJs#$dBc2ckz~l3S zr~(UwF7gMk8E zVNBTr&i47kL(Tv;u5)1+4=FO}M8OHkmr^FkDU$S{T)B!^6r#IU;$@&Y^RAV}OeB4p z5oUiVh$3$PPZ@hbRORbMk>)bq$SbRdRYG?5s}kN+<`e~;z7@G{VKg`!nXNFI z8qSQ@EBp$hi+^o1qR3I-?44iUTswMhy`!o=I_S;nxFSzd8tKq+D@XM2xZ4>0W=#wo zSCUFyyL_|b&UA{qovrmpmA4bcDx<Z$;n{wRtEpiC0000< KMNUMnLSTZlq03qT literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html new file mode 100644 index 0000000000..e8027c4898 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - polynomial.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:5210350.5 %
Date:2024-01-07 21:47:14Functions:61154.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Polynomial::offsetPolynomial(double)0
eth_trajectory_generation::Polynomial::computeMinMax(double, double, int, std::pair<double, double>*, std::pair<double, double>*) const0
eth_trajectory_generation::Polynomial::selectMinMaxFromRoots(double, double, int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::pair<double, double>*, std::pair<double, double>*) const0
eth_trajectory_generation::Polynomial::selectMinMaxFromCandidates(std::vector<double, std::allocator<double> > const&, int, std::pair<double, double>*, std::pair<double, double>*) const0
eth_trajectory_generation::Polynomial::getPolynomialWithAppendedCoefficients(int, eth_trajectory_generation::Polynomial*) const0
eth_trajectory_generation::computeBaseCoefficients(int)57
eth_trajectory_generation::Polynomial::scalePolynomialInTime(double)912
eth_trajectory_generation::Polynomial::convolve(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)2736
eth_trajectory_generation::Polynomial::selectMinMaxCandidatesFromRoots(double, double, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::vector<double, std::allocator<double> >*)4104
eth_trajectory_generation::Polynomial::computeMinMaxCandidates(double, double, int, std::vector<double, std::allocator<double> >*) const4104
eth_trajectory_generation::Polynomial::getRoots(int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*) const4104
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html new file mode 100644 index 0000000000..3da4cf7ee3 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - polynomial.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:5210350.5 %
Date:2024-01-07 21:47:14Functions:61154.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Polynomial::offsetPolynomial(double)0
eth_trajectory_generation::Polynomial::scalePolynomialInTime(double)912
eth_trajectory_generation::Polynomial::selectMinMaxCandidatesFromRoots(double, double, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::vector<double, std::allocator<double> >*)4104
eth_trajectory_generation::Polynomial::convolve(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)2736
eth_trajectory_generation::computeBaseCoefficients(int)57
eth_trajectory_generation::Polynomial::computeMinMax(double, double, int, std::pair<double, double>*, std::pair<double, double>*) const0
eth_trajectory_generation::Polynomial::selectMinMaxFromRoots(double, double, int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::pair<double, double>*, std::pair<double, double>*) const0
eth_trajectory_generation::Polynomial::computeMinMaxCandidates(double, double, int, std::vector<double, std::allocator<double> >*) const4104
eth_trajectory_generation::Polynomial::selectMinMaxFromCandidates(std::vector<double, std::allocator<double> > const&, int, std::pair<double, double>*, std::pair<double, double>*) const0
eth_trajectory_generation::Polynomial::getPolynomialWithAppendedCoefficients(int, eth_trajectory_generation::Polynomial*) const0
eth_trajectory_generation::Polynomial::getRoots(int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*) const4104
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html new file mode 100644 index 0000000000..ed014467f9 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html new file mode 100644 index 0000000000..28e7af816b --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html @@ -0,0 +1,325 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - polynomial.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:5210350.5 %
Date:2024-01-07 21:47:14Functions:61154.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a87c96c5061be37e1ef3e5e2269271e2ce3928cc GIT binary patch literal 1168 zcmV;B1aJF^P)0{{R3u754n0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpo(kL0Z@CqV@W~ zKLIBK&)wg)Re=bu;4+}Uwq9VdA^%NOfE`>-#s=__)?z~@c;e3rW#}Vt^_AI3aNs83 z3dvSo`jLzUi3;!uM{hlW)^-Q02o~7b(!nu^7hW@w1qB);d_^}29uy)tO^_EUlAZa!Xm5I9fyEo4zU!W>Pv_p|FO!l7 z+WhB|7ITLnpd8UeVo)^c(osjCa*^aedZ*KTiGpJ*nsQ(>>Mo#*&Py7lnqGG<1R?cK zays9todq{?lq4~mLSsFg|3bg=uGrS zTHhoA=XmSyaXXGzQg4@g-2Bo!@c}~|*O^5Ws8L8+5ru;%^>F|tsia^;LUK2ONg=gR zKF{Mgu2Kr8P&Y$Se6x_Mem(%uA1R*KN@-?^B_v_jIHa{x(JARQgJ1Bs^8;m?zE+`W zP_*<^JN~~}I3o!c^le7IZ#dESfLJQF4R_k)mc^uex-J|C`X~$y4IUj7l^Br;$D_;D zacro~E{wjtxnkUbhzj*9(3c{SLx#Hp9*Ko(Mwr5p-PCYll_RCGwV2Eepa~go)2z>}20)uiSm)-c^H*1QEQ(My47b8`eXA-2UjcMcC57}Tp4BT4-)uW{~d?bvIeIP95cRp@7 z!iL5*v%x&tnbBY`#UpVCr3L3ZAIk3-HkD04pIt6sX+z_> zujDwqw4~P)>0632rV3zc1N`ZurB%waJi*rmc$m3cv2XptMrFvt8^&Lhg)?9Jv^slg z(`W+n?KL#Xt%fIdDMQ^%Y_D(I8R%_Yvu^vVv(Dazal?k;^$dF{Ph}h!0otu#RmKw) z>h$3Ryv_(%k)sZ*sP)9)1DXKv>)OI8J2dspeX4A+ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
rpoly_ak1.cpp +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
<unnamed>91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html new file mode 100644 index 0000000000..45ec92f979 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
rpoly_ak1.cpp +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
<unnamed>91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html new file mode 100644 index 0000000000..1d23042ada --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
rpoly_ak1.cpp +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
<unnamed>91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html new file mode 100644 index 0000000000..19f62ea907 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
rpoly_ak1.cpp +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html new file mode 100644 index 0000000000..c78064f588 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
rpoly_ak1.cpp +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html new file mode 100644 index 0000000000..710d7032f2 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
rpoly_ak1.cpp +
91.3%91.3%
+ 		91.3 %355 / 389100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html new file mode 100644 index 0000000000..4bb351c2f9 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly - rpoly_ak1.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::rpoly_impl::rpoly_ak1(double*, int*, double*, double*)4104
eth_trajectory_generation::rpolyWrapper(double*, int*, double*, double*)4104
eth_trajectory_generation::findLastNonZeroCoeff(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)4104
eth_trajectory_generation::findRootsJenkinsTraub(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*)4104
eth_trajectory_generation::rpoly_impl::QuadIT_ak1(int, int*, double, double, double*, double*, double*, double*, double*, int, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*)11701
eth_trajectory_generation::rpoly_impl::RealIT_ak1(int*, int*, double*, int, double*, int, double*, double*, double*, double*, double*)11958
eth_trajectory_generation::rpoly_impl::Fxshfr_ak1(int, int*, double, double, double*, int, double*, int, double*, double*, double*, double*, double*)18917
eth_trajectory_generation::rpoly_impl::Quad_ak1(double, double, double, double*, double*, double*, double*)38428
eth_trajectory_generation::rpoly_impl::newest_ak1(int, double*, double*, double, double, double, double, double, double, double, double, double, double, double, double, double*, int, double*)69624
eth_trajectory_generation::rpoly_impl::nextK_ak1(int, int, double, double, double, double*, double*, double*, double*, double*)69714
eth_trajectory_generation::rpoly_impl::calcSC_ak1(int, double, double, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double, double, double*)115214
eth_trajectory_generation::rpoly_impl::QuadSD_ak1(int, double, double, double*, double*, double*, double*)169082
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html new file mode 100644 index 0000000000..54fdee16e1 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly - rpoly_ak1.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::rpoly_impl::Fxshfr_ak1(int, int*, double, double, double*, int, double*, int, double*, double*, double*, double*, double*)18917
eth_trajectory_generation::rpoly_impl::QuadIT_ak1(int, int*, double, double, double*, double*, double*, double*, double*, int, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*)11701
eth_trajectory_generation::rpoly_impl::QuadSD_ak1(int, double, double, double*, double*, double*, double*)169082
eth_trajectory_generation::rpoly_impl::RealIT_ak1(int*, int*, double*, int, double*, int, double*, double*, double*, double*, double*)11958
eth_trajectory_generation::rpoly_impl::calcSC_ak1(int, double, double, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double, double, double*)115214
eth_trajectory_generation::rpoly_impl::newest_ak1(int, double*, double*, double, double, double, double, double, double, double, double, double, double, double, double, double*, int, double*)69624
eth_trajectory_generation::rpoly_impl::Quad_ak1(double, double, double, double*, double*, double*, double*)38428
eth_trajectory_generation::rpoly_impl::nextK_ak1(int, int, double, double, double, double*, double*, double*, double*, double*)69714
eth_trajectory_generation::rpoly_impl::rpoly_ak1(double*, int*, double*, double*)4104
eth_trajectory_generation::rpolyWrapper(double*, int*, double*, double*)4104
eth_trajectory_generation::findLastNonZeroCoeff(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)4104
eth_trajectory_generation::findRootsJenkinsTraub(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*)4104
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html new file mode 100644 index 0000000000..79860f19eb --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html new file mode 100644 index 0000000000..570aa774cc --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html @@ -0,0 +1,1027 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly - rpoly_ak1.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:35538991.3 %
Date:2024-01-07 21:47:14Functions:1212100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..676b0b5347c3dc2b1f53c2fceeb29cf32ea60640 GIT binary patch literal 4794 zcmV;r5=HHaP)61V<(woqQ53@ zF{%6)Lthac-P5y_Uc2m)RvI?!X{UeY2YOp8#Kk+PVfy&i`aL%c_;0rM~o z5|G9uO)gK;N6kW7;4;R-#AZ@?fC4$wr+dOQdYg_#E(A#P^jwls7z@^3O&>87wysG z3tlzx@oYMK*)z;NEDPIQF~H$MY#BN!o{FqW%;EgHE22aIl+ULk%9CYC-b+gXWMaB= zR9SgXQ>KjqMtgK83Bd3i?_0KV{7d_w$7K6yP4-6GRYPO&6jI$zk284GVGWDLYW3Jh z5k>GawDy=jUdlA>dADujXMQ*5mD{w|kU8)2x2jE-?_L2;DwtB2yUNhpw&^Ul=kVL} zCs%WL4!v*HBrwpFbbq zgq$=a1Xtr`HJZ_wTO@ zRRH@PDc~Rt)iqgKX6Y(;>0>f3DJsfr=RI8kZUU9Nv^JC}zZ?%+u6hF?mf*~h_p7^@ z0RfOpr|XI(U(2b2@C@7-?3`tE6Y#vY0l;8m^^p&`Ed)HvZ|?DuV=e%Laxc!{l}^|M z1-ZdyeHN=0bL!v{kaS!w>(2YNL{ZIkv7zuzX22MJzF>2$z@M`lz5y6)gCvaY>!tXI zZi5W>By?Pcf5B@02P^SmMX{n6pM@eJ3aDWq zjWNQIPi7O)M|&ZXsz(VlsZ+quPkt1@**-{!lppQTb#Lcs$j}-VvwbH_%Fb~)4{3lq zLnn>?lv$V_XThKumHSUsk7fcd3pXUxSQz4K7Ua3$?|3+cQfW+F0YFP zuw{B6bL-ls8Zrx2O>gzX{tC!8xR{c7JS@NpkbZcU#}nJ`Z_Y6@KCH%m+CX{JHwtW&+GF=E=*@nR+aCSKeMe-~hh2>dhD_*QQJXJapmjgY;q1iTul> zI79m8M4IjoZyc?mG!LN1BcNS>Tz3m<_&GzK&~L{_i|?6e@CM znssCknLKnbno6m-bcdrrO>8ZjR+U`)TRoBy9?RuWt->vfS?g`sWHzDkIgtd?;d zP{R~`0farX(U#qs9S`+LT;}Z8Ayh6i5|9bz__YKi1DyXli$M)5UbCw($pb@c@^fJ` z0Qp)xphwp$)Hy~HmSXoy^~FbgSx6n`o=goLZ$#|X8yfa?~sp( zJsx4r%b*VR2vec68r2G0Zx0HG`+Aen!|U_#NP7X}Hvp$2_#s6Nsj5auqVqvVTdL5u z0pbmD1XMxHpD|hti$cN}KLXB|kC8@m%*}k_8$H%|Tq0#lJ|45t-}dQ)Rnvay8&l9T zJB+9eIofTngg*m{*WB;%aD6gyMw5=JJ}}W1;7AjjLge?;vHOTPPnjI(pkSwi=oj>a z07)vkc`i^U(qmx1vV5eRey`s`!XlLO&i3YhY1yUI9Uwt0Y{7M{+j1oz{Rc&DFQ!uo!naX4L`4)lgp_P+j&O;1CIYo$#Jk zkaD{jJ_bd7X9*#Hs&}szqs-G^3^WV-u9vY&4Hn#np0!woIV7Yw;OcV^lu>bLS~7Le zR49lkh-}JpmADRmj71s%L;-}F*cX5lbbb8&{yrX$5|_P>eR6pA@$>t8z;>gqrOUqp z*ePQnK#IIJ2DN~He?JeWCz6$*dTera0d3g}S=fH46OXo5!#fz839 z_AYu%$e8k}On8YYZzGj}tArJOz!e@Q(l4BU=5ck^a%jPi7w&=SsP z&Nop}x8O_lM%~o@@57x-V#ryM%9ui&-yh>j0jh;kDT(nB&Xq2O4|-l)yhW8+5@|Bf zdACWvT1*eF{Mt~FMYUgIeC(aNYmyX*z~hXlDGWs61f5i*T^*RDh6yEzoa{RZw(C$5 z5c9hSm*8#8st<@f<{e`nCm@g_$ZE>rHzVv}+hu`6>N%s7?|9HdRa-ylGg9hyRLz9| z$uaYtGyZe#*DBG_1n32?C?^%m0z{gg-i5Yb8Yda0f+T}LAIHtq|3@vU6qIf_$AjE( zu*XO>1=S!i^PsYk#K;-_6WkuI-U9=^lp;-Qa5d87Zu{B{lQXLi6;uThL&IfcN0r14 zTc5dS<){u)|LkXR}FVTJD7#0BzpofYyg&{cvabDgK zU~JWsy>ElPU_A!Fx#~e$Qz!+=+*U?l2ykFA3(heNpUQ45M#R07E(in07PD$H{z^90 zJ?>Ono<*3M;&?fo3jDxU`P@z`LkB{d5`OTqLt_ynH(5B;BhaI2(B9nh4l^Cyg1_B4 z5}^Wsp~aLQ1EmEf6u+V<7U6hJDxHQlCfutPG=?56z2aDMHcY>fbU;dD%SPsIn4p;e zb*q(9clq~`ln-M@JQXn(_Dx=oDt5tjmn+a?Kaes+8D6#)FzP`h4R7k{A~qs@QDQ3_ zM`v?xSahh;g{#iJ!@IGji&1HTYo0iITonmJ!TO8Yu=je#2d^e1DWp_tYgNZ#j{ccf z4DHE#{R;HiW8F-LZP5cWV6Hh#oEmbn`QT!$^kB?o3t68Ii?HYN%qrEBm3=orA~W zl*WDNL1Gdanb~Z>B^j$Upl0Ij7+(*&?ae~Xar0Wt>eC5~@mM3F+Yd{@ymsR=LyHlv z#p9n+cYvXzKtq~qI8$+37O>7#NFYfgMf|LnmazYVBQFRi zWj?W@7m~(qBnh-blDTAA2jjS%;;}102mU(&Up|G9b8q+qa1B(jjsV`y;l7>6J&Mfs z3QogquL}0`!CuI4Jc5Mhq|b|EBSn&OVh+FPZ5AAd9;xlIwfa)&7GLH?kECfuTVa-Z zc2IA2j*PTuAnyHX*@k<$N)#vaStt=N2utQXni_(FGesijynqBfHm-?rd{=*HT>rLZ z(b^L3w)_0Zh?~qh7ePF5!$Vzy!1;BjsNqGeYn$IYu@yY!U$43s@tshpRFN7vEZ&2B zJ?63VbEJ;60J`y6)1$YTlGMzo=2fwKaWP*3^rEB}J+=TgrDw9fuiJqHQB|i$+!4n$6 zeqt~_bSaYvXm(UT1OS80Tu@{Dyd76-8bChtaR^zb1}WGkb1>z%C!I-#)7V`h9v^zt zS-3IsfH*<-5YjjaAsu)8dSP;zdg`u$IItKFuK9Bu0Sp{YRlxS$D*_m6Q}B=mr+hJX zR)Kofa|0%q8XtTFP+QG)QO(5%B{-jt`}j4DGcGu>2_yTuSYgE;yJfo$O9~MeY#pAf zPo+@O9Xj6bhy+#bs}+?TkJd+{=BkRyb1*|ax=nnAYrkv8AIjL)vc5}@)?qwa$K`g7 z?Lt$?A-79kTq*sQcJ%;*kX#6a)=xn2@$1~XPU-;EYp3>3q5TxDfL`@B0QVrG0y=Y) zvw-dmRl$bTmEqXW_>lrbd+AOzu^#{JEKPnxfNMCLo-)Sg*&unAhiBL zC=QY*{(8`aEg~uh8A;k%2%>>vhg~2j23C%EFag{ zrNE~;dq#bhAaD)HvO@H~*4dNF%M$PvLxryAHA*bL1iKiio|iHfNUhz-udxcY;&3@= z9|H^z9nmy{JsJt=T!dySf$WbtfCWG{u*B?x0ao`Ix(IEkktPeUO3`+Ao?}0yKl#-M zSBc(z0rWr(Qmjp?XYifVK!95%v&f?L&MAAl>yF=rIZ?oCAzp#+xR-q4JPSlIZ0KaF1V1eoA#RJN-7^ zyKm;J6Gi*cU(PrB4~%`d@^H-h-x*Ec4ey^%FNTR+mW(eZ<}WLK>sAg87C3>*-3Fzb zTsFu(fook?oH}=F)5faof=9`AKkhj+wBFb52;;RX1?&$k9?4Bbh;Sf%z+jKpPA*q> zzonHh$4ELg>f{a}{9c#zf5b0j%Nrr-*H!1>ce}W?Jr*!R!Z$rl<4)8i{v+u5uRP5m z68>;0WZwEkv(T* z`p9y=@;x;tsq|Sj^4A<6dtc##yEfBl=%`{Jshzo94Vz$yiNn8qx%`SE0b2n@zhtc} z1r2>tt$f|CcO8jV3^UXy&0;DMyn_pu%u9?ZLQ zROfYm(;0iA%)jMdoBaAYajyO~WRfJ<3ngtmZgo5~Ar~lnm#=p|#)kU$znOT(85h0J z;N=MTGHC+RUG_2?MHBge*EkvPvgBYgUjDgv7KC~X8GEIqx)diq_%i+a=t?Ec#=hzF z=&@VcID+uO#qeM0Os$L3vzAOb(LKZ8ToON7OG + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - segment.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:5212740.9 %
Date:2024-01-07 21:47:14Functions:61346.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..40fd4130ce0ea43a66ba68b929f4f1ccaac06d09 GIT binary patch literal 1506 zcmV<81s(c{P)GAU`EmxJcx zFa9&}!QgXxElU%K;0CS&!guolHVclwmn?o#Q_$57pUSg&aSoM)3^yN z{NR9UdN5dML=pXv;@_L(70JFhZ7*xrnx^%2hMr*v+(rlj4!)B>y?XwH!fjjUFRAwb zwZh-d$BbSLJ#MyGfwWEvr*Hb|Sry7g;4#*=ZPupwwmvWoX{Ar#lLRVln4RJh_;;M* zNw=uh>j1B?!@?YPT9zl5m-LWV>%FnwC1Ct1qF|BV_r{`$gJGfI5h&^Sa1zV)c)iB? zd}1;)+6SH{ldMkes_G!15(lM%3i#n1j$1O?Wce}RD9SZ zZY8rle#A-;4t!O>qjYJHC>1XYoY%C)#5qSFywB7I6pcXo82V+VL`2XL`{7z_XYJOB zTr^>^VF#GZ&Vb1<+~W2ny+>D*l2dfJleX54nD-whw*nW|KAf2{QNMFBh-moExnBss zy3CL3+7pC?ggwio^s?w?x|K%XL1ONhWXDHC0;h_KOMNN!h1B312jnAC>qP$M4$>2w zs!jl&V))|=`zTXDg9Y~c(V;eYHGtgj`oo@T3LdEFv+3!E)Foq^VdS)@Zno?qBWZ~f znYnK8K_Z1}W*gT%cUIS=h>M(dH`zxpm!%6tVT6Wo(hSu0Qq+WMydLo{HC;Hp*Zov1 z)nr1odX$-h9XHLx#f}Hp$VzvBDJ9rZydZ zwz-wW^5aEmv6lS1F&0kB=fy|D2_S7L!;Vd?)c)P^#Mu940&ux|iB0kj6Q%VTMB8ov z>;MGNU-#GkYJ=_Flf{ld(y2y%!Yc^zpdh{t)RXaECR?>0_3j0?RBF?e^TOE=~dOb@6$c!hgXp|hITJZ8r$EHuM);9S=u zg2sXbYJ1J + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - timing.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01110.0 %
Date:2024-01-07 21:47:14Functions:0300.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2f4fd270567baa761be463cdc93a69a9924d1442 GIT binary patch literal 1087 zcmV-F1i<@=P)Z0lnW9N1_Hw;hw~? zpKb>rwd<7JmSwAAl}o{^1eKU7q)3w&!7y`d(*7LYV{5s^ZW(~E-Fz0~l)84`bCRz*| z4tziMqgHq+U1A!Cm>EP>_6(D7*rq@lfUPMuMimBVYEhN3t7F$iuxe^5VT`uODx^dn zAWuI0RWo!vx4L2F+N8&YYeKLH8&>%2K>%yT7S2prNJH)^G%@5cWHScg8&=l?JX2+Q zpgf0UyfS~9@b#&AzS&cW@HtR1Q0gSY7b{$|DEB$sDW2h(2h;8`k$-j!;Y$Uc37>5F zE^3SGH?qa9(*toE+SEWc;a!*b9w>=oxNRNAYwCD>Olt+xRvv^#VB71iHdzyaM zgdV%-etE1STQ{+d@`%!N6pzc6nvNafbqcA+md}^RQF=sq{PJ9ZKkJR_%&N#Hc*ITt z{zpA-o5ji_N$0_**TXo>*zu}&l5`K(l_8hw;8mUuzn`&!fBRs?9F&|Zq zk9_ziXY0#Il&{m7m%0FO68#Za216)=!d`uP#&s`g6i78g0!zR57Q<}x_?#_SWE_6- zS~JEnWwdYDE5_XwZLug%prz*PQVg~`YiKA6Sc)=4%3_F`W`#8f&Y%yo>Bf*$xB9>-zEu?T+Q(f~-PRI6d93_!UE$#y?lVw!SVUm7maD<@MT36RGXx zOaZ6KQN0WM + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18229861.1 %
Date:2024-01-07 21:47:14Functions:112347.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
eth_trajectory_generation::Trajectory::offsetTrajectory(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)0
eth_trajectory_generation::Trajectory::scaleSegmentTimes(double)0
eth_trajectory_generation::Trajectory::getVertices(int, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
eth_trajectory_generation::Trajectory::getVertices(int, int, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
eth_trajectory_generation::Trajectory::getGoalVertex(int) const0
eth_trajectory_generation::Trajectory::getStartVertex(int) const0
eth_trajectory_generation::Trajectory::addTrajectories(std::vector<eth_trajectory_generation::Trajectory, std::allocator<eth_trajectory_generation::Trajectory> > const&, eth_trajectory_generation::Trajectory*) const0
eth_trajectory_generation::Trajectory::getVertexAtTime(double, int) const0
eth_trajectory_generation::Trajectory::getTrajectoryWithSingleDimension(int) const0
eth_trajectory_generation::Trajectory::getTrajectoryWithAppendedDimension(eth_trajectory_generation::Trajectory const&, eth_trajectory_generation::Trajectory*) const0
eth_trajectory_generation::Trajectory::evaluate(double, int) const0
eth_trajectory_generation::Trajectory::operator==(eth_trajectory_generation::Trajectory const&) const0
eth_trajectory_generation::Trajectory::scaleSegmentTimesToMeetConstraints(double, double, double, double, double, double, double, double, double)14
eth_trajectory_generation::Trajectory::getSegmentTimes() const14
eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*) const14
eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*) const14
eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*) const14
eth_trajectory_generation::Trajectory::evaluateRange(double, double, double, int, std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >*, std::vector<double, std::allocator<double> >*) const70
eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*) const126
eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*, int) const228
eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*, int) const228
eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*, int) const228
eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*, int) const2052
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html new file mode 100644 index 0000000000..4f0d4451fd --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18229861.1 %
Date:2024-01-07 21:47:14Functions:112347.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ac035620cef3e6b9aba7fd6a15820de45e4b1c7f GIT binary patch literal 2540 zcmVg;1+NPx&_^V-Tzi@fe;d&!RnmP-ZJM*r!#*;UL+VE!!!unI8N_2 zzQ4>rW!@-wZ~sqI76{-BE&{s0jc>4;fc~U1zzQzMV*%JDwVHqkym9XmVf>eZ-bbb* z#etJDfAPF!yYAvKX(|Kkq3E=yK->0*QK1GV)-@Ui@XAT1BBua_;7c%*LKS)SO#T(D zI0X;Mcb$X2HtO^F}f~ zapr^A04AMa!dLVQqaI?!ntFm7Yh$cKYUhq>0%?k4TQ~?$1}P^H33r72Ae5!Ugm-0y z>GSChwsoCX-WKz1cCYrue654#ZRU9Qx-41`8b3;C+TYCXr+Hosg_vTo8H%onfxKma z@~*ucv(rln7}Ce98vY_6Zyb1eNYTVB#v)~@rRs_EwJOCQ+cg-zp>XRUi}xzyOF5;# zd0d%gJUjZ64k8^kF(R<%gXp)f7`QsFYHOn;o1?J1yeW{jJD11D z$IVhMSprF`T&sV=S?HtAiJDYndfx%;?RrjS5(614eGibL6>K8)QWKpN*a~fS1l)vu zZa+))tSg4+;Ob|z!y3Q|x;aJ5`P zgNjBp6+`9ETu-rHS}fOAsf!<7bHvB&$6ZI)()^)? z`Gb|Y%WYn7m==ojhZg1!p)ev3cSzIwzset47@K{K)V-_3W>P|4)A6t{ps<+w1Rnq0 zHYs_#GksjO(jBZ286`403HA{(4b3(_+7)eL={a^HT5Fz5)ErPg?+Nj@3KSCTn58_y z)|qk`FATv8d%mt`1qsRn8&xKsXJid(DUN@vret>SK#2slb7pokArMur!XX5|gNT`T zgL2`H3$EqD5b$+`mjga^c&3h51U#_(xLe_D8FJrW#~o!HnE14wTnADuENL((oO+cM zSayzEmZiTkD_pfPHWMkr);F?=Fw~lN@3O_6(#*l#h6TTq9I2C9bIatnlog$8L&?6l z70_2*cR#QX__s%_C*|1+Md`Ty4TUPj8-_w|EbR1`VI^1Kw~)E)DFjX=9yv5V?jG}* zQ&=7)8-X6RUU=DYfYjCp`_Wq{+C5A6^wFQQpij7{~S)>sS%zyBNw+CmP?gpQ{shx{Y~;HJT4g$?I~(oc5INWXUn+ovv!_Z8s!kloRg0<+72 zSbD`JtGRbU%pbV$2k8`?yzs~VC%SXr)o)?P`HZDvyA%D?$);>Q_!O|K>v?T{@1n?0 zDHNLRdJ!yjbpU&$E*zt{9oWU=D%k1rtm6JJQv4!Cp!kav6<`;S>$gUVk7vbKL<$-x zI=tr*k)n&@F97@@u#3m_7XWG$e*vHf?4tM!0DlPV;&J`b0Py*8*98HJn{HgC$?P z(o|h9xn;>0?@NY@_X@W_g{AXjtF2F2ImP(=O#ZIFt{bXct;QZ#WJeXFGd@y^=)2<-u7}4<$HU|k zfau!vviOnsL;!mC4KvxuC$vBD&fIk+G6lyidN}=5j~=70Y0u=>^{y8VJlS>rM_uij zvfFEGw6C{XI$Bf97>+cX9Gf>;pyojPbDm-}-7Zpm(-cwETE?S}fk^SB3GH-uiRhJt zStLL^nDf9>JaYa>%XBRP#{5OV9v)Zf1|h61JOG@M+S5(^u6xWtVpsIJT(Nz6i5_kD z=~3DfUAJc9wCmQjw<|uLEfK4P&pw5lq{eQv&7?Vh^<<+h7ASf&l+QgfJy#DwuWvNe)m@{>)YsPN(l@86EnObJe=FBw* z)R}yg1+HtFGYdc&MXhB#>KKR=PnxihGxNe^a%KuF + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory_sampling.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:296942.0 %
Date:2024-01-07 21:47:14Functions:2728.6 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

+
          Line data    Source code
+
+       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/trajectory_sampling.h>
+      22             : 
+      23             : namespace eth_trajectory_generation
+      24             : {
+      25             : 
+      26             : const double kNumNanosecondsPerSecond = 1.e9;
+      27             : 
+      28             : /* sampleTrajectoryAtTime() //{ */
+      29             : 
+      30           0 : bool sampleTrajectoryAtTime(const Trajectory& trajectory, double sample_time, eth_mav_msgs::EigenTrajectoryPoint* state) {
+      31           0 :   CHECK_NOTNULL(state);
+      32           0 :   if (sample_time < trajectory.getMinTime() || sample_time > trajectory.getMaxTime()) {
+      33           0 :     LOG(ERROR) << "Sample time should be within [" << trajectory.getMinTime() << " " << trajectory.getMaxTime() << "] but is " << sample_time;
+      34           0 :     return false;
+      35             :   }
+      36             : 
+      37           0 :   if (trajectory.D() < 3) {
+      38           0 :     LOG(ERROR) << "Dimension has to be at least 3, but is " << trajectory.D();
+      39           0 :     return false;
+      40             :   }
+      41             : 
+      42           0 :   return sampleFlatStateAtTime<Trajectory>(trajectory, sample_time, state);
+      43             : }
+      44             : 
+      45             : //}
+      46             : 
+      47             : /* sampleTrajectoryInRange() //{ */
+      48             : 
+      49          14 : bool sampleTrajectoryInRange(const Trajectory& trajectory, double min_time, double max_time, double sampling_interval,
+      50             :                              eth_mav_msgs::EigenTrajectoryPointVector* states) {
+      51          14 :   CHECK_NOTNULL(states);
+      52          14 :   if (min_time < trajectory.getMinTime() || max_time > trajectory.getMaxTime()) {
+      53           0 :     LOG(ERROR) << "Sample time should be within [" << trajectory.getMinTime() << " " << trajectory.getMaxTime() << "] but is [" << min_time << " " << max_time
+      54           0 :                << "]";
+      55           0 :     return false;
+      56             :   }
+      57             : 
+      58          14 :   if (trajectory.D() < 3) {
+      59           0 :     LOG(ERROR) << "Dimension has to be at least 3, but is " << trajectory.D();
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63          28 :   std::vector<Eigen::VectorXd> position, velocity, acceleration, jerk, snap, yaw, yaw_rate;
+      64             : 
+      65          14 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::POSITION, &position);
+      66          14 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::VELOCITY, &velocity);
+      67          14 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::ACCELERATION, &acceleration);
+      68          14 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::JERK, &jerk);
+      69          14 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::SNAP, &snap);
+      70             : 
+      71          14 :   size_t n_samples = position.size();
+      72             : 
+      73          14 :   states->resize(n_samples);
+      74        1942 :   for (size_t i = 0; i < n_samples; ++i) {
+      75        1928 :     eth_mav_msgs::EigenTrajectoryPoint& state = (*states)[i];
+      76             : 
+      77             :     /* state.degrees_of_freedom = eth_mav_msgs::MavActuation::DOF4; */
+      78        1928 :     state.position_W         = position[i].head<3>();
+      79        1928 :     state.velocity_W         = velocity[i].head<3>();
+      80        1928 :     state.acceleration_W     = acceleration[i].head<3>();
+      81        1928 :     state.jerk_W             = jerk[i].head<3>();
+      82        1928 :     state.snap_W             = snap[i].head<3>();
+      83        1928 :     state.time_from_start_ns = static_cast<int64_t>((min_time + sampling_interval * i) * kNumNanosecondsPerSecond);
+      84        1928 :     if (trajectory.D() == 4) {
+      85        1928 :       state.setFromYaw(position[i](3));
+      86        1928 :       state.setFromYawRate(velocity[i](3));
+      87        1928 :       state.setFromYawAcc(acceleration[i](3));
+      88             :     }
+      89             :     /* else if (trajectory.D() == 6) { */
+      90             :     /*   // overactuated, write quaternion from interpolated rotation vector */
+      91             :     /*   Eigen::Vector3d rot_vec, rot_vec_vel, rot_vec_acc; */
+      92             :     /*   rot_vec = position[i].tail<3>(); */
+      93             :     /*   rot_vec_vel  = velocity[i].tail<3>(); */
+      94             :     /*   rot_vec_acc  = acceleration[i].tail<3>(); */
+      95             :     /*   Eigen::Matrix3d rot_matrix; */
+      96             :     /*   eth_mav_msgs::matrixFromRotationVector(rot_vec, &rot_matrix); */
+      97             :     /*   state.orientation_W_B = Eigen::Quaterniond(rot_matrix); */
+      98             :     /*   state.angular_velocity_W = eth_mav_msgs::omegaFromRotationVector(rot_vec, rot_vec_vel); */
+      99             :     /*   state.angular_acceleration_W = eth_mav_msgs::omegaDotFromRotationVector(rot_vec, rot_vec_vel, rot_vec_acc); */
+     100             :     /*   state.degrees_of_freedom = eth_mav_msgs::MavActuation::DOF6; */
+     101             :     /* } */
+     102             :   }
+     103          14 :   return true;
+     104             : }
+     105             : 
+     106             : //}
+     107             : 
+     108             : /* sampleTrajectoryStartDuration() //{ */
+     109             : 
+     110           0 : bool sampleTrajectoryStartDuration(const Trajectory& trajectory, double start_time, double duration, double sampling_interval,
+     111             :                                    eth_mav_msgs::EigenTrajectoryPointVector* states) {
+     112           0 :   return sampleTrajectoryInRange(trajectory, start_time, start_time + duration, sampling_interval, states);
+     113             : }
+     114             : 
+     115             : //}
+     116             : 
+     117             : /* sampleWholeTrajectory() //{ */
+     118             : 
+     119          14 : bool sampleWholeTrajectory(const Trajectory& trajectory, double sampling_interval, eth_mav_msgs::EigenTrajectoryPoint::Vector* states) {
+     120          14 :   const double min_time = trajectory.getMinTime();
+     121          14 :   const double max_time = trajectory.getMaxTime();
+     122             : 
+     123          14 :   return sampleTrajectoryInRange(trajectory, min_time, max_time, sampling_interval, states);
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* sampleSegmentAtTime() //{ */
+     129             : 
+     130           0 : bool sampleSegmentAtTime(const Segment& segment, double sample_time, eth_mav_msgs::EigenTrajectoryPoint* state) {
+     131           0 :   CHECK_NOTNULL(state);
+     132           0 :   if (sample_time < 0.0 || sample_time > segment.getTime()) {
+     133           0 :     LOG(ERROR) << "Sample time should be within [" << 0.0 << " " << segment.getTime() << "] but is " << sample_time;
+     134           0 :     return false;
+     135             :   }
+     136             : 
+     137           0 :   return sampleFlatStateAtTime<Segment>(segment, sample_time, state);
+     138             : }
+     139             : 
+     140             : //}
+     141             : 
+     142             : /* sampleFlatStateAtTime() //{ */
+     143             : 
+     144             : template <class T>
+     145           0 : bool sampleFlatStateAtTime(const T& type, double sample_time, eth_mav_msgs::EigenTrajectoryPoint* state) {
+     146           0 :   if (type.D() < 3) {
+     147           0 :     LOG(ERROR) << "Dimension has to be 3, 4, or 6 but is " << type.D();
+     148           0 :     return false;
+     149             :   }
+     150             : 
+     151           0 :   Eigen::VectorXd position     = type.evaluate(sample_time, derivative_order::POSITION);
+     152           0 :   Eigen::VectorXd velocity     = type.evaluate(sample_time, derivative_order::VELOCITY);
+     153           0 :   Eigen::VectorXd acceleration = type.evaluate(sample_time, derivative_order::ACCELERATION);
+     154             : 
+     155             :   /* state->degrees_of_freedom = eth_mav_msgs::MavActuation::DOF4; */
+     156           0 :   state->position_W     = position.head(3);
+     157           0 :   state->velocity_W     = velocity.head(3);
+     158           0 :   state->acceleration_W = acceleration.head(3);
+     159           0 :   state->jerk_W         = type.evaluate(sample_time, derivative_order::JERK).head(3);
+     160           0 :   state->snap_W         = type.evaluate(sample_time, derivative_order::SNAP).head(3);
+     161             : 
+     162           0 :   if (type.D() == 4) {
+     163           0 :     state->setFromYaw(position(3));
+     164           0 :     state->setFromYawRate(velocity(3));
+     165           0 :     state->setFromYawAcc(acceleration(3));
+     166             :   }
+     167             :   /* else if (type.D() == 6) { */
+     168             :   /*   // overactuated, write quaternion from interpolated rotation vector */
+     169             :   /*   Eigen::Vector3d rot_vec, rot_vec_vel, rot_vec_acc; */
+     170             :   /*   rot_vec  = position.tail(3); */
+     171             :   /*   rot_vec_vel = velocity.tail(3); */
+     172             :   /*   rot_vec_acc = acceleration.tail(3); */
+     173             :   /*   Eigen::Matrix3d rot_matrix; */
+     174             :   /*   eth_mav_msgs::matrixFromRotationVector(rot_vec, &rot_matrix); */
+     175             :   /*   state->orientation_W_B = Eigen::Quaterniond(rot_matrix); */
+     176             :   /*   state->angular_velocity_W = eth_mav_msgs::omegaFromRotationVector(rot_vec, rot_vec_vel); */
+     177             :   /*   state->angular_acceleration_W = eth_mav_msgs::omegaDotFromRotationVector(rot_vec, rot_vec_vel, rot_vec_acc); */
+     178             :   /*   state->degrees_of_freedom = eth_mav_msgs::MavActuation::DOF6; */
+     179             :   /* } */
+     180             : 
+     181           0 :   state->time_from_start_ns = static_cast<int64_t>(sample_time * kNumNanosecondsPerSecond);
+     182           0 :   return true;
+     183             : }
+     184             : 
+     185             : //}
+     186             : 
+     187             : }  // namespace eth_trajectory_generation
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html new file mode 100644 index 0000000000..b362418f82 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html @@ -0,0 +1,67 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8c01d1873eb304f02b3b89269e8d5936e4c2c5a7 GIT binary patch literal 941 zcmV;e15*5nP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp*nVgr7h_@Y=7>^j@uaOdvTzLWL5OvTe%A=05&U z`U{EI;a@2hh+qa+0pZ@}z*3=pB^lrV$MvxTTpG1hs01(Isu@iDNVL8YA4v*S(%*@F zV@)shu@T7tS2Tk6Bzil1*<|v7BBoPd5HDe5vLXdKB%N}YWO7k-kBVoEqRI^6O*7*~ zVE4{d$(>>3vPO9ly&$)bMX1E=WLplrIiry%>#%!CJ|FWb#a2gy-^5X1j)6EJK4Xbm zXf)ZHI6$SbV9FrMLQ8%Ufab(*FnxxIz-c*>dyKII?PM^%vj&D?!&jTM9KRh*9qlAB z%%NXm((;1oKe3+INTvxtjs8{R`>o$ZG(GPA><;2ZqQ2lc@4w?gBD%t(tFdi zue>useDev-e#ggGa=6y}K5)+yUam6Mku~`cPT^1V$JvmAV^t>Uj^}3pS4K@=2A&yL z{Wt{r?(X~UY-{Yj=1n)l-u;7hf9LyHFYP2_fyEwa_9_Qb&JNT~SAZT$<{JV{K)B4n zIzH<_5M9lr(Vu}md8*jim?Vu&XjuAiKo-ga85%j@UV`ay%Ww@nTC6@QVKZa%>154?PAGKj z^RnhUlIA9B=8|TZ!{I`cnVp$R7l5-2PdP=~Q&sD4E!B? z()ipeLwrya?H7S$ORWY@O0${Lq+gLHX##L=vc>n-kIO*1Vc0+;Fx)8jI%-xe0B7A3 z`RGV>0z5ucI&5lz9ViE6H*^+=n~RT82RG#AB1^PG=QSTyApf0Qi(%;5e3b$7=DX81gH P00000NkvXXu0mjfL@=@_ literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html new file mode 100644 index 0000000000..c78e0b7db2 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - vertex.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13326250.8 %
Date:2024-01-07 21:47:14Functions:61735.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2e86326f990cf8a68c94a3946a8d1f43ced1f95b GIT binary patch literal 2242 zcmV;z2tD_SP)QMv^P)s>f zf&CPnZ$GzlxPq{&OtEO|q8tRKr^ZxHvAb|R`s#IIBG5eppX*xkGp=wZvj!Z~W2Qxk z$9L$FSLrnK5nVwBb{5&W1f{-UxO=lZF-YZYs7l>kHQHGl*k=~L>Wu{nP2~r$%Pf4= zE2D_^ma7P>Zd084yThxy&7ho$`$b_SJ3Ex_D!?|yU5yR3_q(geVr+QUb9I)9w9^81 zvN#Y@t{XQJ#UCFa;SIG+OPAQsJd>VXjy;_U&tnvZv^D$&yw|yje6?Y_+HhtqUEy2x z4_Dn;7)8&W>b*(TH6NFF6h}6L-SFPGIj=Eg(DTeAZiUHz3Vb1k3laPq!!HZsg_5@{ zh{L0(N72mVpL~`ckL9gPC_Wy`gO!C{;I%zIraOm)-{JM0mkhX*33TpxnQZYZxlEn7 z`&$k7nxV&GxNB~Q(kvwpDK4E0Bq3#|xH8+XtO4|fJ#d<)D?Ii1o&r&m$qK8Y^036( zROG*!d%V@Jud=e-ucN~*@ClC#cjf5JukGQ?i?uvT*uslHpm0JBgIDrfXl2W>t|MW zkwom!&!m0QDlO@Tk(6UlE!cn1Nb0h9n=2eAKlmAS?)z)jt*d=9b;B)vdTA)h%My-7 zdBS8?99lrzu0Az%FzEtNP0kRHx%mh=fk1KN0~J{?zfjpDHc)I2Q?Tl-WUHKr_f_2= zQ$mbt<31`#T^6N^sTubS{LRW%;#9MtCJcX+krj=iuLQAg=vtNW>l2-xbD$(3;OVNS zT#(0JR~Qh6ZFra|VC2^F%z{MPCphxs>B*3g(a?Bgoe*muMe1hMi)ok4a)5XSq}Iq6 z12T$v5($W>vnu4#)0EPm4A|EjYMF{6+dhy6{GG99#{D<64d+}eLQBq+y}}nYIF5U; zu~i#SmyjLwr>m4~lDc8|BUpBJA?$^^olo3V?2H4Q%Y*6`1sGe=T(^!jfDan&71l%D z*>Im$x1RVE+S)&ZVV0(FXhPk<5cubt@S0J3zS@yZua^hSuZ0rUM=c{^A7U+RV<*~L z_ol{ax)&36c`-oZkuZ_Vk+qcW@@99vq{S1TEMjB)&IX!YDK;dj0ehWG%Ed_>3{s4_ z5Ov$aE1PVfXIgV0?+h!OCA4u*)NBUKhxQE0foqPV$@msKb{|PopuGPvTZ`9)dCL|p zeJa5yep(E_CWUk{(~H2|3D zR)n#>HhZK3P{+qo@p886m@a_!QWSK8qEIZ;D*_j(hXv>h!t8R8Wz~R(ylR67pudD> z_Fr1n2(*Esq{=~IqHgwPejPYowMuOi_(4@$8?91q>rsl7j9YMA%kS(sMaVu?qS*k& zg)sw+(S^llVbO&xx5Ue09k(wpeejs3DJ>r>Cp`9zn+JM@uhf4^YI~$wTNJzM1t9YN zsO3w?DYiPpb3EG0h~}i!2c!Fs*2N{jMTt~rs&3`j2Y@40FU(+<1!PK8IxdVK#Z?$R zr5+liSo~Os_j5clR;}d{1Mq5hh0$<(bHRvTM~kRza47XiLi>hNDayKeqk5X#xT1Q< zXf-|kKJ|2MVDsv+QJO`ox{JYk(JSbJUE3?z^E?(Y>=hn+f_(zCim82jg>SDQyK5PI zP@tOB4EIw!QoX+JBC=5a+7GK&d@oQKHFxK)zk2+6J+i6OHNieEXCxj9ci1{fG~;e9 zP%KvlwU(h}Vc3##^A|TbcLYABcOAiCjL8_tFLAGHoUQT|uV|?wtFFnu&v5cf;3OQ# z0!O*r5}={8XH4<`0CHXOb28vG82$7X)y%v#vde#0XrA+a z9-#@-$*_>)8*qh`(l$Mc_RE zFH!hgvdwXtehH6%aQ1P+dLEDOG)1a9nWhMrzM`L;rev1~?mJDXmH+=VK-Ca0c5WgI~pu)CP zrxcwNs1Zear + + + + + + 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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
<unnamed>62.4 %643 / 103191.3 %21 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-detail-sort-l.html b/mrs_uav_trajectory_generation/src/index-detail-sort-l.html new file mode 100644 index 0000000000..3e6269cd44 --- /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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
<unnamed>62.4 %643 / 103191.3 %21 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-detail.html b/mrs_uav_trajectory_generation/src/index-detail.html new file mode 100644 index 0000000000..52bc19d69a --- /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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
<unnamed>62.4 %643 / 103191.3 %21 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-sort-f.html b/mrs_uav_trajectory_generation/src/index-sort-f.html new file mode 100644 index 0000000000..1354ede283 --- /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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index-sort-l.html b/mrs_uav_trajectory_generation/src/index-sort-l.html new file mode 100644 index 0000000000..7c3d0d2281 --- /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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/index.html b/mrs_uav_trajectory_generation/src/index.html new file mode 100644 index 0000000000..3f396816e4 --- /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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
mrs_trajectory_generation.cpp +
62.4%62.4%
+ 		62.4 %643 / 103191.3 %21 / 23
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html new file mode 100644 index 0000000000..c1bbd95548 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)2
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)3
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&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, ros::Time const&, double const&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> > const&, bool, bool)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)12
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&)13
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)14
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)14
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()14
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)57
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()57
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)189
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()1753
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&)3556
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)7894
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackControlManagerDiag(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)9593
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackTrackerCmd(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)62198
+
+
+ + + +
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..f5eb57a18e --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)57
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)3
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&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)14
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)2
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&)3556
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)189
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackTrackerCmd(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)62198
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)7894
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)14
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, double const&, bool const&)1
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, ros::Time const&, double const&)3
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&)13
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackControlManagerDiag(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)9593
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)0
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()57
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::checkNaN(Waypoint_t const&)12
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::optimize[abi:cxx11](std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std_msgs::Header_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> > const&, bool, bool)3
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()1753
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::timeLeft()14
+
+
+ + + +
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..8ee73225ba --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html @@ -0,0 +1,2408 @@ + + + + + + + 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:643103162.4 %
Date:2024-01-07 21:47:14Functions:212391.3 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : #include <nodelet/nodelet.h>
+       6             : 
+       7             : #include <mrs_msgs/TrajectoryReferenceSrv.h>
+       8             : #include <mrs_msgs/TrajectoryReference.h>
+       9             : 
+      10             : #include <std_srvs/Trigger.h>
+      11             : 
+      12             : #include <geometry_msgs/Vector3Stamped.h>
+      13             : 
+      14             : #include <mrs_msgs/DynamicsConstraints.h>
+      15             : #include <mrs_msgs/Path.h>
+      16             : #include <mrs_msgs/PathSrv.h>
+      17             : #include <mrs_msgs/GetPathSrv.h>
+      18             : #include <mrs_msgs/TrackerCommand.h>
+      19             : #include <mrs_msgs/MpcPredictionFullState.h>
+      20             : #include <mrs_msgs/Reference.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : 
+      23             : #include <eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h>
+      24             : #include <eth_trajectory_generation/trajectory.h>
+      25             : #include <eth_trajectory_generation/trajectory_sampling.h>
+      26             : 
+      27             : #include <mrs_lib/param_loader.h>
+      28             : #include <mrs_lib/geometry/cyclic.h>
+      29             : #include <mrs_lib/geometry/misc.h>
+      30             : #include <mrs_lib/mutex.h>
+      31             : #include <mrs_lib/batch_visualizer.h>
+      32             : #include <mrs_lib/transformer.h>
+      33             : #include <mrs_lib/utils.h>
+      34             : #include <mrs_lib/scope_timer.h>
+      35             : #include <mrs_lib/attitude_converter.h>
+      36             : #include <mrs_lib/publisher_handler.h>
+      37             : 
+      38             : #include <dynamic_reconfigure/server.h>
+      39             : #include <mrs_uav_trajectory_generation/drsConfig.h>
+      40             : 
+      41             : #include <future>
+      42             : 
+      43             : //}
+      44             : 
+      45             : /* using //{ */
+      46             : 
+      47             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      48             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      49             : using mat3_t = Eigen::Matrix3Xd;
+      50             : 
+      51             : using radians  = mrs_lib::geometry::radians;
+      52             : using sradians = mrs_lib::geometry::sradians;
+      53             : 
+      54             : //}
+      55             : 
+      56             : /* defines //{ */
+      57             : 
+      58             : #define FUTURIZATION_EXEC_TIME_FACTOR 0.5        // [0, 1]
+      59             : #define FUTURIZATION_FIRST_WAYPOINT_FACTOR 0.50  // [0, 1]
+      60             : #define OVERTIME_SAFETY_FACTOR 0.95              // [0, 1]
+      61             : #define OVERTIME_SAFETY_OFFSET 0.01              // [s]
+      62             : #define NLOPT_EXEC_TIME_FACTOR 0.95              // [0, 1]
+      63             : 
+      64             : typedef struct
+      65             : {
+      66             :   Eigen::Vector4d coords;
+      67             :   bool            stop_at;
+      68         880 : } Waypoint_t;
+      69             : 
+      70             : //}
+      71             : 
+      72             : namespace mrs_uav_trajectory_generation
+      73             : {
+      74             : 
+      75             : /* class MrsTrajectoryGeneration //{ */
+      76             : class MrsTrajectoryGeneration : public nodelet::Nodelet {
+      77             : 
+      78             : public:
+      79             :   virtual void onInit();
+      80             : 
+      81             : private:
+      82             :   ros::NodeHandle nh_;
+      83             : 
+      84             :   bool is_initialized_ = false;
+      85             : 
+      86             :   // | ----------------------- parameters ----------------------- |
+      87             : 
+      88             :   double _sampling_dt_;
+      89             : 
+      90             :   double _max_trajectory_len_factor_;
+      91             :   double _min_trajectory_len_factor_;
+      92             : 
+      93             :   int _n_attempts_;
+      94             : 
+      95             :   double _min_waypoint_distance_;
+      96             : 
+      97             :   bool   _fallback_sampling_enabled_;
+      98             :   double _fallback_sampling_speed_factor_;
+      99             :   double _fallback_sampling_accel_factor_;
+     100             :   double _fallback_sampling_stopping_time_;
+     101             :   bool   _fallback_sampling_first_waypoint_additional_stop_;
+     102             : 
+     103             :   std::string _uav_name_;
+     104             : 
+     105             :   bool   _trajectory_max_segment_deviation_enabled_;
+     106             :   double _trajectory_max_segment_deviation_;
+     107             :   int    _trajectory_max_segment_deviation_max_iterations_;
+     108             : 
+     109             :   bool   _path_straightener_enabled_;
+     110             :   double _path_straightener_max_deviation_;
+     111             :   double _path_straightener_max_hdg_deviation_;
+     112             : 
+     113             :   // | -------- variable parameters (come with the path) -------- |
+     114             : 
+     115             :   std::string frame_id_;
+     116             :   bool        fly_now_                              = false;
+     117             :   bool        use_heading_                          = false;
+     118             :   bool        stop_at_waypoints_                    = false;
+     119             :   bool        override_constraints_                 = false;
+     120             :   bool        loop_                                 = false;
+     121             :   double      override_max_velocity_horizontal_     = 0.0;
+     122             :   double      override_max_velocity_vertical_       = 0.0;
+     123             :   double      override_max_acceleration_horizontal_ = 0.0;
+     124             :   double      override_max_acceleration_vertical_   = 0.0;
+     125             :   double      override_max_jerk_horizontal_         = 0.0;
+     126             :   double      override_max_jerk_vertical_           = 0.0;
+     127             : 
+     128             :   // | -------------- variable parameters (deduced) ------------- |
+     129             : 
+     130             :   double     max_execution_time_ = 0;
+     131             :   std::mutex mutex_max_execution_time_;
+     132             : 
+     133             :   bool max_deviation_first_segment_;
+     134             : 
+     135             :   // | -------------------- the transformer  -------------------- |
+     136             : 
+     137             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     138             : 
+     139             :   // | ------------------- scope timer logger ------------------- |
+     140             : 
+     141             :   bool                                       scope_timer_enabled_ = false;
+     142             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     143             : 
+     144             :   // service client for input
+     145             :   bool               callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res);
+     146             :   ros::ServiceServer service_server_path_;
+     147             : 
+     148             :   // service client for returning result to the user
+     149             :   bool               callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res);
+     150             :   ros::ServiceServer service_server_get_path_;
+     151             : 
+     152             :   // subscriber for input
+     153             :   void            callbackPath(const mrs_msgs::PathConstPtr& msg);
+     154             :   ros::Subscriber subscriber_path_;
+     155             : 
+     156             :   void                          callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg);
+     157             :   ros::Subscriber               subscriber_constraints_;
+     158             :   bool                          got_constraints_ = false;
+     159             :   mrs_msgs::DynamicsConstraints constraints_;
+     160             :   std::mutex                    mutex_constraints_;
+     161             : 
+     162             :   void                     callbackTrackerCmd(const mrs_msgs::TrackerCommandConstPtr& msg);
+     163             :   ros::Subscriber          subscriber_tracker_cmd_;
+     164             :   bool                     got_tracker_cmd_ = false;
+     165             :   mrs_msgs::TrackerCommand tracker_cmd_;
+     166             :   std::mutex               mutex_tracker_cmd_;
+     167             : 
+     168             :   void                                callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg);
+     169             :   ros::Subscriber                     subscriber_control_manager_diag_;
+     170             :   bool                                got_control_manager_diag_ = false;
+     171             :   mrs_msgs::ControlManagerDiagnostics control_manager_diag_;
+     172             :   std::mutex                          mutex_control_manager_diag_;
+     173             : 
+     174             :   // service client for publishing trajectory out
+     175             :   ros::ServiceClient service_client_trajectory_reference_;
+     176             : 
+     177             :   // solve the whole problem
+     178             :   std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> optimize(const std::vector<Waypoint_t>& waypoints_in, const std_msgs::Header& waypoints_stamp,
+     179             :                                                                         const mrs_msgs::TrackerCommand&         initial_condition,
+     180             :                                                                         const mrs_msgs::MpcPredictionFullState& current_prediction, bool fallback_sampling,
+     181             :                                                                         const bool relax_heading);
+     182             : 
+     183             :   // batch vizualizer
+     184             :   mrs_lib::BatchVisualizer bw_original_;
+     185             :   mrs_lib::BatchVisualizer bw_final_;
+     186             : 
+     187             :   // transforming TrackerCommand
+     188             :   std::optional<mrs_msgs::Path> transformPath(const mrs_msgs::Path& path, const std::string& target_frame);
+     189             : 
+     190             :   // | ------------------ trajectory validation ----------------- |
+     191             : 
+     192             :   /**
+     193             :    * @brief validates samples of a trajectory agains a path of waypoints
+     194             :    *
+     195             :    * @param trajectory
+     196             :    * @param segments
+     197             :    *
+     198             :    * @return <success, traj_fail_idx, path_fail_segment>
+     199             :    */
+     200             :   std::tuple<bool, int, std::vector<bool>, double> validateTrajectorySpatial(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+     201             :                                                                              const std::vector<Waypoint_t>&                    waypoints);
+     202             : 
+     203             :   std::vector<int> getTrajectorySegmentCenterIdxs(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector<Waypoint_t>& waypoints);
+     204             : 
+     205             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectory(const std::vector<Waypoint_t>&  waypoints,
+     206             :                                                                            const mrs_msgs::TrackerCommand& initial_state, const double& sampling_dt,
+     207             :                                                                            const bool& relax_heading);
+     208             : 
+     209             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector>              findTrajectoryFallback(const std::vector<Waypoint_t>&  waypoints,
+     210             :                                                                                                 const mrs_msgs::TrackerCommand& initial_state, const double& sampling_dt,
+     211             :                                                                                                 const bool& relax_heading);
+     212             :   std::future<std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector>> future_trajectory_result_;
+     213             :   std::atomic<bool>                                                      running_async_planning_ = false;
+     214             : 
+     215             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectoryAsync(const std::vector<Waypoint_t>&  waypoints,
+     216             :                                                                                 const mrs_msgs::TrackerCommand& initial_state, const double& sampling_dt,
+     217             :                                                                                 const bool& relax_heading);
+     218             : 
+     219             :   std::vector<Waypoint_t> preprocessPath(const std::vector<Waypoint_t>& waypoints_in);
+     220             : 
+     221             :   mrs_msgs::TrajectoryReference getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const ros::Time& stamp,
+     222             :                                                        const double& sampling_dt);
+     223             : 
+     224             :   Waypoint_t interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff);
+     225             : 
+     226             :   bool checkNaN(const Waypoint_t& a);
+     227             : 
+     228             :   double distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2);
+     229             : 
+     230             :   bool trajectorySrv(const mrs_msgs::TrajectoryReference& msg);
+     231             : 
+     232             :   // | --------------- dynamic reconfigure server --------------- |
+     233             : 
+     234             :   boost::recursive_mutex                           mutex_drs_;
+     235             :   typedef mrs_uav_trajectory_generation::drsConfig DrsParams_t;
+     236             :   typedef dynamic_reconfigure::Server<DrsParams_t> Drs_t;
+     237             :   boost::shared_ptr<Drs_t>                         drs_;
+     238             :   void                                             callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, uint32_t level);
+     239             :   DrsParams_t                                      params_;
+     240             :   std::mutex                                       mutex_params_;
+     241             : 
+     242             :   // | ------------ Republisher for the desired path ------------ |
+     243             : 
+     244             :   mrs_lib::PublisherHandler<mrs_msgs::Path> ph_original_path_;
+     245             : 
+     246             :   // | ------------- measuring the time of execution ------------ |
+     247             : 
+     248             :   ros::Time  start_time_total_;
+     249             :   std::mutex mutex_start_time_total_;
+     250             :   bool       overtime(void);
+     251             :   double     timeLeft(void);
+     252             : };
+     253             : 
+     254             : //}
+     255             : 
+     256             : /* onInit() //{ */
+     257             : 
+     258          57 : void MrsTrajectoryGeneration::onInit() {
+     259             : 
+     260             :   /* obtain node handle */
+     261          57 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     262             : 
+     263             :   /* waits for the ROS to publish clock */
+     264          57 :   ros::Time::waitForValid();
+     265             : 
+     266             :   // | ----------------------- publishers ----------------------- |
+     267             : 
+     268         171 :   ph_original_path_ = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "original_path_out", 1);
+     269             : 
+     270             :   // | ----------------------- subscribers ---------------------- |
+     271             : 
+     272          57 :   subscriber_constraints_ = nh_.subscribe("constraints_in", 1, &MrsTrajectoryGeneration::callbackConstraints, this, ros::TransportHints().tcpNoDelay());
+     273          57 :   subscriber_tracker_cmd_ = nh_.subscribe("tracker_cmd_in", 1, &MrsTrajectoryGeneration::callbackTrackerCmd, this, ros::TransportHints().tcpNoDelay());
+     274          57 :   subscriber_control_manager_diag_ =
+     275         114 :       nh_.subscribe("control_manager_diag_in", 1, &MrsTrajectoryGeneration::callbackControlManagerDiag, this, ros::TransportHints().tcpNoDelay());
+     276          57 :   subscriber_path_ = nh_.subscribe("path_in", 1, &MrsTrajectoryGeneration::callbackPath, this, ros::TransportHints().tcpNoDelay());
+     277             : 
+     278             :   // | --------------------- service servers -------------------- |
+     279             : 
+     280          57 :   service_server_path_ = nh_.advertiseService("path_in", &MrsTrajectoryGeneration::callbackPathSrv, this);
+     281             : 
+     282          57 :   service_server_get_path_ = nh_.advertiseService("get_path_in", &MrsTrajectoryGeneration::callbackGetPathSrv, this);
+     283             : 
+     284         114 :   service_client_trajectory_reference_ = nh_.serviceClient<mrs_msgs::TrajectoryReferenceSrv>("trajectory_reference_out");
+     285             : 
+     286             :   // | ----------------------- parameters ----------------------- |
+     287             : 
+     288          57 :   mrs_lib::ParamLoader param_loader(nh_, "MrsTrajectoryGeneration");
+     289             : 
+     290         114 :   std::string custom_config_path;
+     291          57 :   std::string platform_config_path;
+     292             : 
+     293          57 :   param_loader.loadParam("custom_config", custom_config_path);
+     294          57 :   param_loader.loadParam("platform_config", platform_config_path);
+     295             : 
+     296          57 :   if (custom_config_path != "") {
+     297          57 :     param_loader.addYamlFile(custom_config_path);
+     298             :   }
+     299             : 
+     300          57 :   if (platform_config_path != "") {
+     301          57 :     param_loader.addYamlFile(platform_config_path);
+     302             :   }
+     303             : 
+     304          57 :   param_loader.addYamlFileFromParam("private_config");
+     305          57 :   param_loader.addYamlFileFromParam("public_config");
+     306             : 
+     307         114 :   const std::string yaml_prefix = "mrs_uav_trajectory_generation/";
+     308             : 
+     309          57 :   param_loader.loadParam("uav_name", _uav_name_);
+     310             : 
+     311         114 :   param_loader.loadParam(yaml_prefix + "sampling_dt", _sampling_dt_);
+     312             : 
+     313         114 :   param_loader.loadParam(yaml_prefix + "enforce_fallback_solver", params_.enforce_fallback_solver);
+     314             : 
+     315         114 :   param_loader.loadParam(yaml_prefix + "max_trajectory_len_factor", _max_trajectory_len_factor_);
+     316         114 :   param_loader.loadParam(yaml_prefix + "min_trajectory_len_factor", _min_trajectory_len_factor_);
+     317             : 
+     318         114 :   param_loader.loadParam(yaml_prefix + "n_attempts", _n_attempts_);
+     319         114 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/enabled", _fallback_sampling_enabled_);
+     320         114 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/speed_factor", _fallback_sampling_speed_factor_);
+     321         114 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/accel_factor", _fallback_sampling_accel_factor_);
+     322         114 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/stopping_time", _fallback_sampling_stopping_time_);
+     323         114 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/first_waypoint_additional_stop", _fallback_sampling_first_waypoint_additional_stop_);
+     324             : 
+     325         114 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/enabled", _trajectory_max_segment_deviation_enabled_);
+     326         114 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_deviation", _trajectory_max_segment_deviation_);
+     327         114 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_iterations", _trajectory_max_segment_deviation_max_iterations_);
+     328             : 
+     329         114 :   param_loader.loadParam(yaml_prefix + "path_straightener/enabled", _path_straightener_enabled_);
+     330         114 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_deviation", _path_straightener_max_deviation_);
+     331         114 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_hdg_deviation", _path_straightener_max_hdg_deviation_);
+     332             : 
+     333         114 :   param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_);
+     334             : 
+     335             :   // | --------------------- tf transformer --------------------- |
+     336             : 
+     337          57 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "TrajectoryGeneration");
+     338          57 :   transformer_->setDefaultPrefix(_uav_name_);
+     339          57 :   transformer_->retryLookupNewest(true);
+     340             : 
+     341             :   // | ------------------- scope timer logger ------------------- |
+     342             : 
+     343         114 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     344         171 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     345          57 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     346             : 
+     347             :   // | --------------------- service clients -------------------- |
+     348             : 
+     349         114 :   param_loader.loadParam(yaml_prefix + "time_penalty", params_.time_penalty);
+     350         114 :   param_loader.loadParam(yaml_prefix + "soft_constraints_enabled", params_.soft_constraints_enabled);
+     351         114 :   param_loader.loadParam(yaml_prefix + "soft_constraints_weight", params_.soft_constraints_weight);
+     352         114 :   param_loader.loadParam(yaml_prefix + "time_allocation", params_.time_allocation);
+     353         114 :   param_loader.loadParam(yaml_prefix + "equality_constraint_tolerance", params_.equality_constraint_tolerance);
+     354         114 :   param_loader.loadParam(yaml_prefix + "inequality_constraint_tolerance", params_.inequality_constraint_tolerance);
+     355         114 :   param_loader.loadParam(yaml_prefix + "max_iterations", params_.max_iterations);
+     356         114 :   param_loader.loadParam(yaml_prefix + "derivative_to_optimize", params_.derivative_to_optimize);
+     357             : 
+     358         114 :   param_loader.loadParam(yaml_prefix + "max_time", params_.max_time);
+     359          57 :   max_execution_time_ = params_.max_time;
+     360             : 
+     361          57 :   if (!param_loader.loadedSuccessfully()) {
+     362           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: could not load all parameters!");
+     363           0 :     ros::shutdown();
+     364             :   }
+     365             : 
+     366             :   // | -------------------- batch visualizer -------------------- |
+     367             : 
+     368         114 :   bw_original_ = mrs_lib::BatchVisualizer(nh_, "markers/original", "");
+     369             : 
+     370          57 :   bw_original_.clearBuffers();
+     371          57 :   bw_original_.clearVisuals();
+     372             : 
+     373          57 :   bw_final_ = mrs_lib::BatchVisualizer(nh_, "markers/final", "");
+     374             : 
+     375          57 :   bw_final_.clearBuffers();
+     376          57 :   bw_final_.clearVisuals();
+     377             : 
+     378             :   // | --------------- dynamic reconfigure server --------------- |
+     379             : 
+     380          57 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     381          57 :   drs_->updateConfig(params_);
+     382         114 :   Drs_t::CallbackType f = boost::bind(&MrsTrajectoryGeneration::callbackDrs, this, _1, _2);
+     383          57 :   drs_->setCallback(f);
+     384             : 
+     385             :   // | --------------------- finish the init -------------------- |
+     386             : 
+     387         114 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: initialized");
+     388             : 
+     389          57 :   is_initialized_ = true;
+     390          57 : }
+     391             : 
+     392             : //}
+     393             : 
+     394             : // | ---------------------- main routines --------------------- |
+     395             : 
+     396             : /*
+     397             :  * 1. preprocessPath(): preprocessing the incoming path
+     398             :  *    - throughs away too close waypoints
+     399             :  *    - straightness path by neglecting waypoints close to segments
+     400             :  * 2. optimize(): solves the whole problem including
+     401             :  *    - subdivision for satisfying max deviation
+     402             :  * 3. findTrajectory(): solves single instance by the ETH tool
+     403             :  * 4. findTrajectoryFallback(): Baca's sampling for backup solution
+     404             :  * 5. validateTrajectorySpatial(): checks for the spatial soundness of a trajectory vs. the original path
+     405             :  */
+     406             : 
+     407             : /* preprocessPath() //{ */
+     408             : 
+     409           3 : std::vector<Waypoint_t> MrsTrajectoryGeneration::preprocessPath(const std::vector<Waypoint_t>& waypoints_in) {
+     410             : 
+     411           6 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::preprocessPath", scope_timer_logger_, scope_timer_enabled_);
+     412             : 
+     413           3 :   std::vector<Waypoint_t> waypoints;
+     414             : 
+     415           3 :   size_t last_added_idx = 0;  // in "waypoints_in"
+     416             : 
+     417          18 :   for (size_t i = 0; i < waypoints_in.size(); i++) {
+     418             : 
+     419          15 :     double x       = waypoints_in.at(i).coords[0];
+     420          15 :     double y       = waypoints_in.at(i).coords[1];
+     421          15 :     double z       = waypoints_in.at(i).coords[2];
+     422          15 :     double heading = waypoints_in.at(i).coords[3];
+     423             : 
+     424          15 :     bw_original_.addPoint(vec3_t(x, y, z), 1.0, 0.0, 0.0, 1.0);
+     425             : 
+     426          15 :     if (_path_straightener_enabled_ && waypoints_in.size() >= 3 && i > 0 && i < (waypoints_in.size() - 1)) {
+     427             : 
+     428           0 :       vec3_t first(waypoints_in.at(last_added_idx).coords[0], waypoints_in.at(last_added_idx).coords[1], waypoints_in.at(last_added_idx).coords[2]);
+     429           0 :       vec3_t last(waypoints_in.at(i + 1).coords[0], waypoints_in.at(i + 1).coords[1], waypoints_in.at(i + 1).coords[2]);
+     430             : 
+     431           0 :       double first_hdg = waypoints_in.at(last_added_idx).coords[3];
+     432           0 :       double last_hdg  = waypoints_in.at(i + 1).coords[3];
+     433             : 
+     434           0 :       size_t next_point = last_added_idx + 1;
+     435             : 
+     436           0 :       bool segment_is_ok = true;
+     437             : 
+     438           0 :       for (size_t j = next_point; j < i + 1; j++) {
+     439             : 
+     440           0 :         vec3_t mid(waypoints_in.at(j).coords[0], waypoints_in.at(j).coords[1], waypoints_in.at(j).coords[2]);
+     441           0 :         double mid_hdg = waypoints_in.at(j).coords[3];
+     442             : 
+     443           0 :         double dist_from_segment = distFromSegment(mid, first, last);
+     444             : 
+     445           0 :         if (dist_from_segment > _path_straightener_max_deviation_ || fabs(radians::diff(first_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_) ||
+     446           0 :             fabs(radians::diff(last_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_)) {
+     447           0 :           segment_is_ok = false;
+     448           0 :           break;
+     449             :         }
+     450             :       }
+     451             : 
+     452           0 :       if (segment_is_ok) {
+     453           0 :         continue;
+     454             :       }
+     455             :     }
+     456             : 
+     457          15 :     if (i > 0 && i < (waypoints_in.size() - 1)) {
+     458             : 
+     459           9 :       vec3_t first(waypoints_in.at(last_added_idx).coords[0], waypoints_in.at(last_added_idx).coords[1], waypoints_in.at(last_added_idx).coords[2]);
+     460           9 :       vec3_t last(waypoints_in.at(i).coords[0], waypoints_in.at(i).coords[1], waypoints_in.at(i).coords[2]);
+     461             : 
+     462           9 :       if (mrs_lib::geometry::dist(first, last) < _min_waypoint_distance_) {
+     463           0 :         ROS_INFO("[MrsTrajectoryGeneration]: waypoint #%d too close (< %.3f m) to the previous one (#%d), throwing it away", int(i), _min_waypoint_distance_,
+     464             :                  int(last_added_idx));
+     465           0 :         continue;
+     466             :       }
+     467             :     }
+     468             : 
+     469          15 :     Waypoint_t wp;
+     470          15 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+     471          15 :     wp.stop_at = waypoints_in.at(i).stop_at;
+     472          15 :     waypoints.push_back(wp);
+     473             : 
+     474          15 :     last_added_idx = i;
+     475             :   }
+     476             : 
+     477           3 :   return waypoints;
+     478             : }
+     479             : 
+     480             : //}
+     481             : 
+     482             : /* optimize() //{ */
+     483             : 
+     484           3 : std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> MrsTrajectoryGeneration::optimize(const std::vector<Waypoint_t>&          waypoints_in,
+     485             :                                                                                                const std_msgs::Header&                 waypoints_header,
+     486             :                                                                                                const mrs_msgs::TrackerCommand&         tracker_cmd,
+     487             :                                                                                                const mrs_msgs::MpcPredictionFullState& current_prediction,
+     488             :                                                                                                const bool fallback_sampling, const bool relax_heading) {
+     489             : 
+     490           9 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::optimize", scope_timer_logger_, scope_timer_enabled_);
+     491             : 
+     492           3 :   ros::Time optimize_time_start = ros::Time::now();
+     493             : 
+     494             :   // | ------------- prepare the initial conditions ------------- |
+     495             : 
+     496           6 :   mrs_msgs::TrackerCommand initial_condition;
+     497             : 
+     498           3 :   bool path_from_future = false;
+     499             : 
+     500             :   // positive = in the future
+     501           3 :   double path_time_offset = 0;
+     502             : 
+     503           3 :   if (waypoints_header.stamp != ros::Time(0)) {
+     504           0 :     path_time_offset = (waypoints_header.stamp - ros::Time::now()).toSec();
+     505             :   }
+     506             : 
+     507           3 :   int path_sample_offset = 0;
+     508             : 
+     509             :   // if the desired path starts in the future, more than one MPC step ahead
+     510           3 :   if (path_time_offset > 0.2) {
+     511             : 
+     512           0 :     ROS_INFO("[MrsTrajectoryGeneration]: desired path is from the future by %.2f s", path_time_offset);
+     513             : 
+     514             :     // calculate the offset in samples in the predicted trajectory
+     515             :     // 0.01 is subtracted for the first sample, which is smaller
+     516             :     // +1 is added due to the first sample, which was subtarcted
+     517           0 :     path_sample_offset = int(ceil((path_time_offset * FUTURIZATION_FIRST_WAYPOINT_FACTOR - 0.01) / 0.2)) + 1;
+     518             : 
+     519           0 :     if (path_sample_offset > (int(current_prediction.position.size()) - 1)) {
+     520             : 
+     521           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: can not extrapolate into the waypoints, using tracker_cmd instead");
+     522           0 :       initial_condition = tracker_cmd;
+     523             : 
+     524             :     } else {
+     525             : 
+     526             :       // copy the sample from the current prediction into TrackerCommand, so that we can easily transform it
+     527           0 :       mrs_msgs::TrackerCommand full_state;
+     528             : 
+     529           0 :       full_state.header = current_prediction.header;
+     530             : 
+     531           0 :       full_state.position     = current_prediction.position[path_sample_offset];
+     532           0 :       full_state.velocity     = current_prediction.velocity[path_sample_offset];
+     533           0 :       full_state.acceleration = current_prediction.acceleration[path_sample_offset];
+     534           0 :       full_state.jerk         = current_prediction.jerk[path_sample_offset];
+     535             : 
+     536           0 :       full_state.heading              = current_prediction.heading[path_sample_offset];
+     537           0 :       full_state.heading_rate         = current_prediction.heading_rate[path_sample_offset];
+     538           0 :       full_state.heading_acceleration = current_prediction.heading_acceleration[path_sample_offset];
+     539           0 :       full_state.heading_jerk         = current_prediction.heading_jerk[path_sample_offset];
+     540             : 
+     541           0 :       ROS_INFO("[MrsTrajectoryGeneration]: getting initial condition from the %d-th sample of the MPC prediction", path_sample_offset);
+     542             : 
+     543           0 :       initial_condition.header = full_state.header;
+     544             : 
+     545           0 :       initial_condition.position     = full_state.position;
+     546           0 :       initial_condition.velocity     = full_state.velocity;
+     547           0 :       initial_condition.acceleration = full_state.acceleration;
+     548           0 :       initial_condition.jerk         = full_state.jerk;
+     549             : 
+     550           0 :       initial_condition.heading              = full_state.heading;
+     551           0 :       initial_condition.heading_rate         = full_state.heading_rate;
+     552           0 :       initial_condition.heading_acceleration = full_state.heading_acceleration;
+     553           0 :       initial_condition.heading_jerk         = full_state.heading_jerk;
+     554             : 
+     555           0 :       path_from_future = true;
+     556             :     }
+     557             : 
+     558             :   } else {
+     559             : 
+     560           6 :     ROS_INFO("[MrsTrajectoryGeneration]: desired path is NOT from the future, using tracker_cmd as the initial condition");
+     561             : 
+     562           3 :     initial_condition = tracker_cmd;
+     563             :   }
+     564             : 
+     565           3 :   auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_);
+     566             : 
+     567           3 :   if (waypoints_header.stamp == ros::Time(0)) {
+     568           3 :     if (!control_manager_diag.tracker_status.have_goal) {
+     569           0 :       initial_condition.header.stamp = ros::Time(0);
+     570             :     }
+     571             :   }
+     572             : 
+     573             :   // | ---------------- reset the visual markers ---------------- |
+     574             : 
+     575           3 :   bw_original_.clearBuffers();
+     576           3 :   bw_original_.clearVisuals();
+     577           3 :   bw_final_.clearBuffers();
+     578           3 :   bw_final_.clearVisuals();
+     579             : 
+     580           3 :   bw_original_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     581           3 :   bw_final_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     582             : 
+     583           3 :   bw_original_.setPointsScale(0.4);
+     584           3 :   bw_final_.setPointsScale(0.35);
+     585             : 
+     586             :   // empty path is invalid
+     587           3 :   if (waypoints_in.size() == 0) {
+     588           0 :     std::stringstream ss;
+     589           0 :     ss << "the path is empty (before postprocessing)";
+     590           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     591           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     592             :   }
+     593             : 
+     594           6 :   std::vector<Waypoint_t> waypoints_in_with_init = waypoints_in;
+     595             : 
+     596           3 :   if (path_time_offset > 0.2 && waypoints_in_with_init.size() >= 2) {
+     597           0 :     waypoints_in_with_init.erase(waypoints_in_with_init.begin());
+     598             :   }
+     599             : 
+     600             :   // prepend the initial condition
+     601           3 :   Waypoint_t initial_waypoint;
+     602           3 :   initial_waypoint.coords =
+     603           3 :       Eigen::Vector4d(initial_condition.position.x, initial_condition.position.y, initial_condition.position.z, initial_condition.heading);
+     604           3 :   initial_waypoint.stop_at = false;
+     605           3 :   waypoints_in_with_init.insert(waypoints_in_with_init.begin(), initial_waypoint);
+     606             : 
+     607           6 :   std::vector<Waypoint_t> waypoints = preprocessPath(waypoints_in_with_init);
+     608             : 
+     609           3 :   if (waypoints.size() <= 1) {
+     610           0 :     std::stringstream ss;
+     611           0 :     ss << "the path is empty (after postprocessing)";
+     612           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     613           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     614             :   }
+     615             : 
+     616           3 :   bool              safe = false;
+     617           3 :   int               traj_idx;
+     618           6 :   std::vector<bool> segment_safeness;
+     619           3 :   double            max_deviation = 0;
+     620             : 
+     621           6 :   eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory;
+     622             : 
+     623           3 :   double sampling_dt = 0;
+     624             : 
+     625           3 :   if (path_from_future) {
+     626           0 :     ROS_INFO("[MrsTrajectoryGeneration]: changing dt = 0.2, cause the path is from the future");
+     627           0 :     sampling_dt = 0.2;
+     628             :   } else {
+     629           3 :     sampling_dt = _sampling_dt_;
+     630             :   }
+     631             : 
+     632           3 :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> result;
+     633             : 
+     634           6 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+     635             : 
+     636           3 :   if (params.enforce_fallback_solver) {
+     637           0 :     ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced");
+     638           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     639           3 :   } else if (fallback_sampling) {
+     640           0 :     ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling");
+     641           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     642           3 :   } else if (running_async_planning_) {
+     643           0 :     ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     644           0 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     645           3 :   } else if (overtime()) {
+     646           2 :     ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time");
+     647           3 :     result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     648             :   } else {
+     649           6 :     result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     650             :   }
+     651             : 
+     652           3 :   if (result) {
+     653           3 :     trajectory = result.value();
+     654             :   } else {
+     655           0 :     std::stringstream ss;
+     656           0 :     ss << "failed to find trajectory";
+     657           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     658           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     659             :   }
+     660             : 
+     661          15 :   for (int k = 0; k < _trajectory_max_segment_deviation_max_iterations_; k++) {
+     662             : 
+     663          16 :     ROS_DEBUG("[MrsTrajectoryGeneration]: revalidation cycle #%d", k);
+     664             : 
+     665          13 :     std::tie(safe, traj_idx, segment_safeness, max_deviation) = validateTrajectorySpatial(trajectory, waypoints);
+     666             : 
+     667          13 :     if (_trajectory_max_segment_deviation_enabled_ && !safe) {
+     668             : 
+     669          14 :       ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is not safe, max deviation %.3f m", max_deviation);
+     670             : 
+     671          12 :       std::vector<Waypoint_t>::iterator waypoint = waypoints.begin();
+     672          12 :       std::vector<bool>::iterator       safeness = segment_safeness.begin();
+     673             : 
+     674         182 :       for (; waypoint < waypoints.end() - 1; waypoint++) {
+     675             : 
+     676         170 :         if (!(*safeness)) {
+     677             : 
+     678          50 :           if (waypoint > waypoints.begin() || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+     679          50 :             Waypoint_t midpoint2 = interpolatePoint(*waypoint, *(waypoint + 1), 0.5);
+     680          50 :             waypoint             = waypoints.insert(waypoint + 1, midpoint2);
+     681             :           }
+     682             :         }
+     683             : 
+     684         170 :         safeness++;
+     685             :       }
+     686             : 
+     687          12 :       if (params.enforce_fallback_solver) {
+     688           0 :         ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced");
+     689           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     690          12 :       } else if (fallback_sampling) {
+     691           0 :         ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling");
+     692           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     693          12 :       } else if (running_async_planning_) {
+     694           0 :         ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     695           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     696          12 :       } else if (overtime()) {
+     697           0 :         ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time");
+     698           0 :         result = findTrajectoryFallback(waypoints, initial_condition, sampling_dt, relax_heading);
+     699             :       } else {
+     700          36 :         result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     701             :       }
+     702             : 
+     703          12 :       if (result) {
+     704          12 :         trajectory = result.value();
+     705             :       } else {
+     706           0 :         std::stringstream ss;
+     707           0 :         ss << "failed to find trajectory";
+     708           0 :         ROS_WARN_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     709           0 :         return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     710             :       }
+     711             : 
+     712             :     } else {
+     713           2 :       ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is safe (%.2f)", max_deviation);
+     714           1 :       safe = true;
+     715           1 :       break;
+     716             :     }
+     717             :   }
+     718             : 
+     719           6 :   ROS_INFO("[MrsTrajectoryGeneration]: final max trajectory-path deviation: %.2f m, total trajectory time: %.2fs ", max_deviation,
+     720             :            trajectory.size() * sampling_dt);
+     721             : 
+     722             :   // prepare rviz markers
+     723          68 :   for (int i = 0; i < int(waypoints.size()); i++) {
+     724          65 :     bw_final_.addPoint(vec3_t(waypoints.at(i).coords[0], waypoints.at(i).coords[1], waypoints.at(i).coords[2]), 0.0, 1.0, 0.0, 1.0);
+     725             :   }
+     726             : 
+     727           6 :   mrs_msgs::TrajectoryReference mrs_trajectory;
+     728             : 
+     729             :   // convert the optimized trajectory to mrs_msgs::TrajectoryReference
+     730           3 :   mrs_trajectory = getTrajectoryReference(trajectory, initial_condition.header.stamp, sampling_dt);
+     731             : 
+     732             :   // insert part of the MPC prediction in the front of the generated trajectory to compensate for the future
+     733           3 :   if (path_from_future) {
+     734             : 
+     735             :     // calculate the starting idx that we will use from the current_prediction
+     736           0 :     double path_time_offset_2   = (ros::Time::now() - current_prediction.header.stamp).toSec();  // = how long did it take to optimize
+     737           0 :     int    path_sample_offset_2 = int(floor((path_time_offset_2 - 0.01) / 0.2)) + 1;
+     738             : 
+     739             :     // if there is anything to insert
+     740           0 :     if (path_sample_offset > path_sample_offset_2) {
+     741             : 
+     742           0 :       ROS_INFO("[MrsTrajectoryGeneration]: inserting pre-trajectory from the prediction, idxs %d to %d", path_sample_offset_2, path_sample_offset);
+     743             : 
+     744           0 :       for (int i = path_sample_offset - 1; i >= 0; i--) {
+     745             : 
+     746           0 :         ROS_DEBUG("[MrsTrajectoryGeneration]: inserting idx %d", i);
+     747             : 
+     748           0 :         mrs_msgs::ReferenceStamped reference;
+     749             : 
+     750           0 :         reference.header = current_prediction.header;
+     751             : 
+     752           0 :         reference.reference.heading  = current_prediction.heading[i];
+     753           0 :         reference.reference.position = current_prediction.position[i];
+     754             : 
+     755           0 :         auto res = transformer_->transformSingle(reference, waypoints_header.frame_id);
+     756             : 
+     757           0 :         if (res) {
+     758           0 :           reference = res.value();
+     759             :         } else {
+     760           0 :           std::stringstream ss;
+     761           0 :           ss << "could not transform reference to the path frame";
+     762           0 :           ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+     763           0 :           return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     764             :         }
+     765             : 
+     766           0 :         mrs_trajectory.points.insert(mrs_trajectory.points.begin(), reference.reference);
+     767             :       }
+     768             :     }
+     769             :   }
+     770             : 
+     771           3 :   bw_original_.publish();
+     772           3 :   bw_final_.publish();
+     773             : 
+     774           6 :   std::stringstream ss;
+     775           3 :   ss << "trajectory generated";
+     776             : 
+     777           6 :   ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory generated, took %.3f s", (ros::Time::now() - optimize_time_start).toSec());
+     778             : 
+     779           6 :   return std::tuple(true, ss.str(), mrs_trajectory);
+     780             : }
+     781             : 
+     782             : //}
+     783             : 
+     784             : /* findTrajectory() //{ */
+     785             : 
+     786          14 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectory(const std::vector<Waypoint_t>&  waypoints,
+     787             :                                                                                                   const mrs_msgs::TrackerCommand& initial_state,
+     788             :                                                                                                   const double& sampling_dt, const bool& relax_heading) {
+     789             : 
+     790          42 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectory", scope_timer_logger_, scope_timer_enabled_);
+     791             : 
+     792          14 :   mrs_lib::AtomicScopeFlag unset_running(running_async_planning_);
+     793             : 
+     794          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: findTrajectory() started");
+     795             : 
+     796          14 :   ros::Time find_trajectory_time_start = ros::Time::now();
+     797             : 
+     798          28 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+     799          14 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     800             : 
+     801          28 :   auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_);
+     802             : 
+     803          14 :   if ((initial_state.header.stamp - ros::Time::now()).toSec() < 0.2 && control_manager_diag.tracker_status.have_goal) {
+     804           6 :     max_deviation_first_segment_ = false;
+     805             :   } else {
+     806           8 :     max_deviation_first_segment_ = true;
+     807             :   }
+     808             : 
+     809             :   // optimizer
+     810             : 
+     811          14 :   eth_trajectory_generation::NonlinearOptimizationParameters parameters;
+     812             : 
+     813          14 :   parameters.f_rel                  = 0.05;
+     814          14 :   parameters.x_rel                  = 0.1;
+     815          14 :   parameters.time_penalty           = params.time_penalty;
+     816          14 :   parameters.use_soft_constraints   = params.soft_constraints_enabled;
+     817          14 :   parameters.soft_constraint_weight = params.soft_constraints_weight;
+     818          14 :   parameters.time_alloc_method      = static_cast<eth_trajectory_generation::NonlinearOptimizationParameters::TimeAllocMethod>(params.time_allocation);
+     819          14 :   if (params.time_allocation == 2) {
+     820          14 :     parameters.algorithm = nlopt::LD_LBFGS;
+     821             :   }
+     822          14 :   parameters.initial_stepsize_rel            = 0.1;
+     823          14 :   parameters.inequality_constraint_tolerance = params.inequality_constraint_tolerance;
+     824          14 :   parameters.equality_constraint_tolerance   = params.equality_constraint_tolerance;
+     825          14 :   parameters.max_iterations                  = params.max_iterations;
+     826          14 :   parameters.max_time                        = NLOPT_EXEC_TIME_FACTOR * timeLeft();
+     827             : 
+     828          14 :   eth_trajectory_generation::Vertex::Vector vertices;
+     829          14 :   const int                                 dimension = 4;
+     830             : 
+     831          14 :   int derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     832             : 
+     833          14 :   switch (params.derivative_to_optimize) {
+     834             :     case 0: {
+     835             :       derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     836             :       break;
+     837             :     }
+     838           0 :     case 1: {
+     839           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::JERK;
+     840           0 :       break;
+     841             :     }
+     842           0 :     case 2: {
+     843           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::SNAP;
+     844           0 :       break;
+     845             :     }
+     846             :   }
+     847             : 
+     848             :   // | --------------- add constraints to vertices -------------- |
+     849             : 
+     850          14 :   double last_heading = initial_state.heading;
+     851             : 
+     852         256 :   for (size_t i = 0; i < waypoints.size(); i++) {
+     853         242 :     double x       = waypoints.at(i).coords[0];
+     854         242 :     double y       = waypoints.at(i).coords[1];
+     855         242 :     double z       = waypoints.at(i).coords[2];
+     856         242 :     double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading);
+     857         242 :     last_heading   = heading;
+     858             : 
+     859         484 :     eth_trajectory_generation::Vertex vertex(dimension);
+     860             : 
+     861         242 :     if (i == 0) {
+     862             : 
+     863          28 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     864             : 
+     865          28 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     866             : 
+     867          14 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY,
+     868          14 :                            Eigen::Vector4d(initial_state.velocity.x, initial_state.velocity.y, initial_state.velocity.z, initial_state.heading_rate));
+     869             : 
+     870          14 :       vertex.addConstraint(
+     871             :           eth_trajectory_generation::derivative_order::ACCELERATION,
+     872          14 :           Eigen::Vector4d(initial_state.acceleration.x, initial_state.acceleration.y, initial_state.acceleration.z, initial_state.heading_acceleration));
+     873             : 
+     874          14 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK,
+     875          28 :                            Eigen::Vector4d(initial_state.jerk.x, initial_state.jerk.y, initial_state.jerk.z, initial_state.heading_jerk));
+     876             : 
+     877         228 :     } else if (i == (waypoints.size() - 1)) {  // the last point
+     878             : 
+     879          28 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     880             : 
+     881          28 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     882             : 
+     883             :     } else {  // mid points
+     884             : 
+     885         428 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     886             : 
+     887         214 :       if (waypoints.at(i).stop_at) {
+     888           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector4d(0, 0, 0, 0));
+     889           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, Eigen::Vector4d(0, 0, 0, 0));
+     890           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, Eigen::Vector4d(0, 0, 0, 0));
+     891             :       }
+     892             :     }
+     893             : 
+     894         242 :     vertices.push_back(vertex);
+     895             :   }
+     896             : 
+     897             :   // | ---------------- compute the segment times --------------- |
+     898             : 
+     899          14 :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+     900          14 :   double v_max_vertical, a_max_vertical, j_max_vertical;
+     901             : 
+     902             :   // use the small of the ascending/descending values
+     903          14 :   double vertical_speed_lim        = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed);
+     904          14 :   double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration);
+     905             : 
+     906          14 :   v_max_horizontal = constraints.horizontal_speed;
+     907          14 :   a_max_horizontal = constraints.horizontal_acceleration;
+     908             : 
+     909          14 :   v_max_vertical = vertical_speed_lim;
+     910          14 :   a_max_vertical = vertical_acceleration_lim;
+     911             : 
+     912          14 :   j_max_horizontal = constraints.horizontal_jerk;
+     913          14 :   j_max_vertical   = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk);
+     914             : 
+     915          14 :   if (override_constraints_) {
+     916             : 
+     917           0 :     bool can_change = (hypot(initial_state.velocity.x, initial_state.velocity.y) < override_max_velocity_horizontal_) &&
+     918           0 :                       (hypot(initial_state.acceleration.x, initial_state.acceleration.y) < override_max_acceleration_horizontal_) &&
+     919           0 :                       (hypot(initial_state.jerk.x, initial_state.jerk.y) < override_max_jerk_horizontal_) &&
+     920           0 :                       (fabs(initial_state.velocity.z) < override_max_velocity_vertical_) &&
+     921           0 :                       (fabs(initial_state.acceleration.z) < override_max_acceleration_vertical_) && (fabs(initial_state.jerk.z) < override_max_jerk_vertical_);
+     922             : 
+     923           0 :     if (can_change) {
+     924             : 
+     925           0 :       v_max_horizontal = override_max_velocity_horizontal_;
+     926           0 :       a_max_horizontal = override_max_acceleration_horizontal_;
+     927           0 :       j_max_horizontal = override_max_jerk_horizontal_;
+     928             : 
+     929           0 :       v_max_vertical = override_max_velocity_vertical_;
+     930           0 :       a_max_vertical = override_max_acceleration_vertical_;
+     931           0 :       j_max_vertical = override_max_jerk_vertical_;
+     932             : 
+     933           0 :       ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user");
+     934             : 
+     935             :     } else {
+     936             : 
+     937           0 :       ROS_WARN("[MrsTrajectoryGeneration]: overrifing constraints refused due to possible infeasibility");
+     938             :     }
+     939             :   }
+     940             : 
+     941          14 :   double v_max_heading, a_max_heading, j_max_heading;
+     942             : 
+     943          14 :   if (relax_heading) {
+     944             :     v_max_heading = std::numeric_limits<float>::max();
+     945             :     a_max_heading = std::numeric_limits<float>::max();
+     946             :     j_max_heading = std::numeric_limits<float>::max();
+     947             :   } else {
+     948          14 :     v_max_heading = constraints.heading_speed;
+     949          14 :     a_max_heading = constraints.heading_acceleration;
+     950          14 :     j_max_heading = constraints.heading_jerk;
+     951             :   }
+     952             : 
+     953          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:");
+     954          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+     955          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+     956          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+     957             : 
+     958          42 :   std::vector<double> segment_times, segment_times_baca;
+     959          14 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+     960          14 :                                        v_max_heading, a_max_heading);
+     961          14 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+     962          14 :                                                 v_max_heading, a_max_heading);
+     963             : 
+     964          14 :   double initial_total_time      = 0;
+     965          14 :   double initial_total_time_baca = 0;
+     966         242 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+     967         228 :     initial_total_time += segment_times[i];
+     968         228 :     initial_total_time_baca += segment_times_baca[i];
+     969             :   }
+     970             : 
+     971          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Euclidean): %.2f", initial_total_time);
+     972          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Baca): %.2f", initial_total_time_baca);
+     973             : 
+     974             :   // | --------- create an optimizer object and solve it -------- |
+     975             : 
+     976          14 :   const int                                                     N = 10;
+     977          28 :   eth_trajectory_generation::PolynomialOptimizationNonLinear<N> opt(dimension, parameters);
+     978          14 :   opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
+     979             : 
+     980          14 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+     981          14 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+     982          14 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+     983             : 
+     984          14 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+     985          14 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+     986          14 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+     987             : 
+     988          14 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::VELOCITY, v_max_vertical);
+     989          14 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_vertical);
+     990          14 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::JERK, j_max_vertical);
+     991             : 
+     992          14 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::VELOCITY, v_max_heading);
+     993          14 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_heading);
+     994          14 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::JERK, j_max_heading);
+     995             : 
+     996          14 :   opt.optimize();
+     997             : 
+     998          14 :   if (overtime()) {
+     999           0 :     return {};
+    1000             :   }
+    1001             : 
+    1002          28 :   std::string result_str;
+    1003             : 
+    1004          28 :   switch (opt.getOptimizationInfo().stopping_reason) {
+    1005           3 :     case nlopt::FAILURE: {
+    1006           3 :       result_str = "generic failure";
+    1007             :       break;
+    1008             :     }
+    1009           0 :     case nlopt::INVALID_ARGS: {
+    1010           0 :       result_str = "invalid args";
+    1011             :       break;
+    1012             :     }
+    1013           0 :     case nlopt::OUT_OF_MEMORY: {
+    1014           0 :       result_str = "out of memory";
+    1015             :       break;
+    1016             :     }
+    1017           0 :     case nlopt::ROUNDOFF_LIMITED: {
+    1018           0 :       result_str = "roundoff limited";
+    1019             :       break;
+    1020             :     }
+    1021           0 :     case nlopt::FORCED_STOP: {
+    1022           0 :       result_str = "forced stop";
+    1023             :       break;
+    1024             :     }
+    1025           0 :     case nlopt::STOPVAL_REACHED: {
+    1026           0 :       result_str = "stopval reached";
+    1027             :       break;
+    1028             :     }
+    1029           8 :     case nlopt::FTOL_REACHED: {
+    1030           8 :       result_str = "ftol reached";
+    1031             :       break;
+    1032             :     }
+    1033           1 :     case nlopt::XTOL_REACHED: {
+    1034           1 :       result_str = "xtol reached";
+    1035             :       break;
+    1036             :     }
+    1037           2 :     case nlopt::MAXEVAL_REACHED: {
+    1038           2 :       result_str = "maxeval reached";
+    1039             :       break;
+    1040             :     }
+    1041           0 :     case nlopt::MAXTIME_REACHED: {
+    1042           0 :       result_str = "maxtime reached";
+    1043             :       break;
+    1044             :     }
+    1045           0 :     default: {
+    1046           0 :       result_str = "UNKNOWN FAILURE CODE";
+    1047             :       break;
+    1048             :     }
+    1049             :   }
+    1050             : 
+    1051          50 :   if (opt.getOptimizationInfo().stopping_reason >= 1 && opt.getOptimizationInfo().stopping_reason != 6) {
+    1052          13 :     ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished successfully with code %d, '%s'", opt.getOptimizationInfo().stopping_reason,
+    1053             :               result_str.c_str());
+    1054             : 
+    1055           6 :   } else if (opt.getOptimizationInfo().stopping_reason == -1) {
+    1056           5 :     ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished with a generic error code %d, '%s'", opt.getOptimizationInfo().stopping_reason,
+    1057             :               result_str.c_str());
+    1058             : 
+    1059             :   } else {
+    1060           0 :     ROS_WARN("[MrsTrajectoryGeneration]: optimization failed with code %d, '%s', took %.3f s", opt.getOptimizationInfo().stopping_reason, result_str.c_str(),
+    1061             :              (ros::Time::now() - find_trajectory_time_start).toSec());
+    1062          14 :     return {};
+    1063             :   }
+    1064             : 
+    1065             :   // | ------------- obtain the polynomial segments ------------- |
+    1066             : 
+    1067          14 :   eth_trajectory_generation::Segment::Vector segments;
+    1068          14 :   opt.getPolynomialOptimizationRef().getSegments(&segments);
+    1069             : 
+    1070          14 :   if (overtime()) {
+    1071           0 :     return {};
+    1072             :   }
+    1073             : 
+    1074             :   // | --------------- create the trajectory class -------------- |
+    1075             : 
+    1076          28 :   eth_trajectory_generation::Trajectory trajectory;
+    1077          14 :   opt.getTrajectory(&trajectory);
+    1078             : 
+    1079          28 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1080             : 
+    1081          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: starting eth sampling with dt = %.2f s ", sampling_dt);
+    1082             : 
+    1083          14 :   bool success = eth_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_dt, &states);
+    1084             : 
+    1085          14 :   if (overtime()) {
+    1086           0 :     return {};
+    1087             :   }
+    1088             : 
+    1089             :   // validate the temporal sampling of the trajectory
+    1090             : 
+    1091             :   // only check this if the trajectory is > 1.0 sec, this check does not make much sense for the short ones
+    1092          14 :   if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) > (_max_trajectory_len_factor_ * initial_total_time_baca)) {
+    1093           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too long = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1094             :               (states.size() * sampling_dt), initial_total_time_baca, _max_trajectory_len_factor_);
+    1095             : 
+    1096           0 :     std::stringstream ss;
+    1097           0 :     ss << "trajectory sampling failed";
+    1098           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+    1099           0 :     return {};
+    1100             : 
+    1101          14 :   } else if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) < (_min_trajectory_len_factor_ * initial_total_time_baca)) {
+    1102           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too short = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1103             :               (states.size() * sampling_dt), initial_total_time_baca, _min_trajectory_len_factor_);
+    1104             : 
+    1105           0 :     std::stringstream ss;
+    1106           0 :     ss << "trajectory sampling failed";
+    1107           0 :     ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+    1108           0 :     return {};
+    1109             : 
+    1110             :   } else {
+    1111          16 :     ROS_DEBUG("[MrsTrajectoryGeneration]: estimated/final trajectory length ratio (final/estimated) %.2f",
+    1112             :               (states.size() * sampling_dt) / initial_total_time_baca);
+    1113             :   }
+    1114             : 
+    1115          14 :   if (success) {
+    1116          16 :     ROS_DEBUG("[MrsTrajectoryGeneration]: eth sampling finished, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1117          14 :     return std::optional(states);
+    1118             : 
+    1119             :   } else {
+    1120           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: eth could not sample the trajectory, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1121          14 :     return {};
+    1122             :   }
+    1123             : }
+    1124             : 
+    1125             : //}
+    1126             : 
+    1127             : /* findTrajectoryFallback() //{ */
+    1128             : 
+    1129           1 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryFallback(const std::vector<Waypoint_t>&  waypoints,
+    1130             :                                                                                                           const mrs_msgs::TrackerCommand& initial_state,
+    1131             :                                                                                                           const double&                   sampling_dt,
+    1132             :                                                                                                           const bool&                     relax_heading) {
+    1133             : 
+    1134           2 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectoryFallback", scope_timer_logger_, scope_timer_enabled_);
+    1135             : 
+    1136           1 :   ros::Time time_start = ros::Time::now();
+    1137             : 
+    1138           2 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling started");
+    1139             : 
+    1140           2 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+    1141           1 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1142             : 
+    1143           2 :   eth_trajectory_generation::Vertex::Vector vertices;
+    1144           1 :   const int                                 dimension = 4;
+    1145             : 
+    1146             :   // | --------------- add constraints to vertices -------------- |
+    1147             : 
+    1148           1 :   double last_heading = initial_state.heading;
+    1149             : 
+    1150           6 :   for (size_t i = 0; i < waypoints.size(); i++) {
+    1151             : 
+    1152           5 :     double x       = waypoints.at(i).coords[0];
+    1153           5 :     double y       = waypoints.at(i).coords[1];
+    1154           5 :     double z       = waypoints.at(i).coords[2];
+    1155           5 :     double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading);
+    1156           5 :     last_heading   = heading;
+    1157             : 
+    1158          10 :     eth_trajectory_generation::Vertex vertex(dimension);
+    1159             : 
+    1160          10 :     vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+    1161             : 
+    1162           5 :     vertices.push_back(vertex);
+    1163             :   }
+    1164             : 
+    1165             :   // | ---------------- compute the segment times --------------- |
+    1166             : 
+    1167           1 :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+    1168           1 :   double v_max_vertical, a_max_vertical, j_max_vertical;
+    1169             : 
+    1170             :   // use the small of the ascending/descending values
+    1171           1 :   double vertical_speed_lim        = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed);
+    1172           1 :   double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration);
+    1173             : 
+    1174           1 :   if (override_constraints_) {
+    1175             : 
+    1176           0 :     v_max_horizontal = override_max_velocity_horizontal_;
+    1177           0 :     a_max_horizontal = override_max_acceleration_horizontal_;
+    1178           0 :     j_max_horizontal = override_max_jerk_horizontal_;
+    1179             : 
+    1180           0 :     v_max_vertical = override_max_velocity_vertical_;
+    1181           0 :     a_max_vertical = override_max_acceleration_vertical_;
+    1182           0 :     j_max_vertical = override_max_jerk_vertical_;
+    1183             : 
+    1184           0 :     ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user");
+    1185             :   } else {
+    1186             : 
+    1187           1 :     v_max_horizontal = constraints.horizontal_speed;
+    1188           1 :     a_max_horizontal = constraints.horizontal_acceleration;
+    1189             : 
+    1190           1 :     v_max_vertical = vertical_speed_lim;
+    1191           1 :     a_max_vertical = vertical_acceleration_lim;
+    1192             : 
+    1193           1 :     j_max_horizontal = constraints.horizontal_jerk;
+    1194           1 :     j_max_vertical   = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk);
+    1195             :   }
+    1196             : 
+    1197             : 
+    1198           1 :   double v_max_heading, a_max_heading, j_max_heading;
+    1199             : 
+    1200           1 :   if (relax_heading) {
+    1201             :     v_max_heading = std::numeric_limits<float>::max();
+    1202             :     a_max_heading = std::numeric_limits<float>::max();
+    1203             :     j_max_heading = std::numeric_limits<float>::max();
+    1204             :   } else {
+    1205           1 :     v_max_heading = constraints.heading_speed;
+    1206           1 :     a_max_heading = constraints.heading_acceleration;
+    1207           1 :     j_max_heading = constraints.heading_jerk;
+    1208             :   }
+    1209             : 
+    1210           1 :   v_max_horizontal *= _fallback_sampling_speed_factor_;
+    1211           1 :   v_max_vertical *= _fallback_sampling_speed_factor_;
+    1212             : 
+    1213           1 :   a_max_horizontal *= _fallback_sampling_accel_factor_;
+    1214           1 :   a_max_vertical *= _fallback_sampling_accel_factor_;
+    1215             : 
+    1216           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:");
+    1217           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+    1218           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+    1219           2 :   ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+    1220             : 
+    1221           3 :   std::vector<double> segment_times, segment_times_baca;
+    1222           1 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1223           1 :                                        v_max_heading, a_max_heading);
+    1224           1 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1225           1 :                                                 v_max_heading, a_max_heading);
+    1226             : 
+    1227           1 :   double initial_total_time      = 0;
+    1228           1 :   double initial_total_time_baca = 0;
+    1229           5 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+    1230           4 :     initial_total_time += segment_times[i];
+    1231           4 :     initial_total_time_baca += segment_times_baca[i];
+    1232             :   }
+    1233             : 
+    1234           2 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time);
+    1235           2 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca);
+    1236             : 
+    1237             :   // | --------- create an optimizer object and solve it -------- |
+    1238             : 
+    1239           2 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1240             : 
+    1241             :   // interpolate each segment
+    1242           5 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1243             : 
+    1244           8 :     Eigen::VectorXd start, end;
+    1245             : 
+    1246           4 :     const double segment_time = segment_times_baca[i];
+    1247             : 
+    1248           4 :     int    n_samples;
+    1249           4 :     double interp_step;
+    1250             : 
+    1251           4 :     if (segment_time > 1e-1) {
+    1252             : 
+    1253           4 :       n_samples = floor(segment_time / sampling_dt);
+    1254             : 
+    1255             :       // important
+    1256           4 :       if (n_samples > 0) {
+    1257           4 :         interp_step = 1.0 / double(n_samples);
+    1258             :       } else {
+    1259             :         interp_step = 0.5;
+    1260             :       }
+    1261             : 
+    1262             :     } else {
+    1263             :       n_samples   = 0;
+    1264             :       interp_step = 0;
+    1265             :     }
+    1266             : 
+    1267             :     // for the last segment, git the last waypoint completely
+    1268             :     // otherwise, it is hit as the first sample of the following segment
+    1269           4 :     if (n_samples > 0 && i == waypoints.size() - 2) {
+    1270           1 :       n_samples++;
+    1271             :     }
+    1272             : 
+    1273         143 :     for (int j = 0; j < n_samples; j++) {
+    1274             : 
+    1275         139 :       Waypoint_t point = interpolatePoint(waypoints[i], waypoints[i + 1], j * interp_step);
+    1276             : 
+    1277         139 :       eth_mav_msgs::EigenTrajectoryPoint eth_point;
+    1278         139 :       eth_point.position_W[0] = point.coords[0];
+    1279         139 :       eth_point.position_W[1] = point.coords[1];
+    1280         139 :       eth_point.position_W[2] = point.coords[2];
+    1281         139 :       eth_point.setFromYaw(point.coords[3]);
+    1282             : 
+    1283         139 :       states.push_back(eth_point);
+    1284             : 
+    1285         278 :       auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_);
+    1286             : 
+    1287             :       // the first sample of the first waypoint
+    1288             :       // we should stop for a little bit to give the transition
+    1289             :       // from the initial cooordinates more time
+    1290         139 :       if (((control_manager_diag.tracker_status.have_goal && i == 0) || (!control_manager_diag.tracker_status.have_goal && i == 1)) && j == 0) {
+    1291             : 
+    1292           1 :         double time_to_stop = 0;
+    1293             : 
+    1294           1 :         time_to_stop += fabs(initial_state.acceleration.x) / j_max_horizontal + fabs(initial_state.acceleration.y) / j_max_horizontal +
+    1295           1 :                         fabs(initial_state.acceleration.z) / j_max_vertical;
+    1296             : 
+    1297           1 :         int samples_to_stop = int(round(1.0 * (time_to_stop / sampling_dt)));
+    1298             : 
+    1299           1 :         if (_fallback_sampling_first_waypoint_additional_stop_) {
+    1300           0 :           if (control_manager_diag.tracker_status.have_goal) {
+    1301           0 :             samples_to_stop += int(round(1.0 / sampling_dt));
+    1302             :           }
+    1303             :         }
+    1304             : 
+    1305           2 :         ROS_DEBUG("[MrsTrajectoryGeneration]: pre-inserting %d samples of the first point", samples_to_stop);
+    1306             : 
+    1307           1 :         for (int k = 0; k < samples_to_stop; k++) {
+    1308           0 :           states.push_back(eth_point);
+    1309             :         }
+    1310             :       }
+    1311             : 
+    1312         139 :       if (j == 0 && i > 0 && waypoints[i].stop_at) {
+    1313             : 
+    1314           0 :         int insert_samples = int(round(_fallback_sampling_stopping_time_ / sampling_dt));
+    1315             : 
+    1316           0 :         for (int k = 0; k < insert_samples; k++) {
+    1317           0 :           states.push_back(eth_point);
+    1318             :         }
+    1319             :       }
+    1320             :     }
+    1321             :   }
+    1322             : 
+    1323           1 :   bool success = true;
+    1324             : 
+    1325           2 :   ROS_WARN("[MrsTrajectoryGeneration]: fallback: sampling finished, took %.3f s", (ros::Time::now() - time_start).toSec());
+    1326             : 
+    1327             :   // | --------------- create the trajectory class -------------- |
+    1328             : 
+    1329           1 :   if (success) {
+    1330           1 :     return std::optional(states);
+    1331             :   } else {
+    1332             :     ROS_ERROR("[MrsTrajectoryGeneration]: fallback: sampling failed");
+    1333           1 :     return {};
+    1334             :   }
+    1335             : }
+    1336             : 
+    1337             : //}
+    1338             : 
+    1339             : /* validateTrajectorySpatial() //{ */
+    1340             : 
+    1341          13 : std::tuple<bool, int, std::vector<bool>, double> MrsTrajectoryGeneration::validateTrajectorySpatial(
+    1342             :     const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector<Waypoint_t>& waypoints) {
+    1343             : 
+    1344          26 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::validateTrajectorySpatial", scope_timer_logger_, scope_timer_enabled_);
+    1345             : 
+    1346             :   // prepare the output
+    1347             : 
+    1348          26 :   std::vector<bool> segments;
+    1349         187 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1350         174 :     segments.push_back(true);
+    1351             :   }
+    1352             : 
+    1353             :   int waypoint_idx = 0;
+    1354             : 
+    1355             :   bool   is_safe       = true;
+    1356             :   double max_deviation = 0;
+    1357             : 
+    1358        1791 :   for (size_t i = 0; i < trajectory.size() - 1; i++) {
+    1359             : 
+    1360             :     // the trajectory sample
+    1361        1778 :     const vec3_t sample = vec3_t(trajectory[i].position_W[0], trajectory[i].position_W[1], trajectory[i].position_W[2]);
+    1362             : 
+    1363             :     // next sample
+    1364        1778 :     const vec3_t next_sample = vec3_t(trajectory[i + 1].position_W[0], trajectory[i + 1].position_W[1], trajectory[i + 1].position_W[2]);
+    1365             : 
+    1366             :     // segment start
+    1367        1778 :     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]);
+    1368             : 
+    1369             :     // segment end
+    1370        1778 :     const vec3_t segment_end =
+    1371        1778 :         vec3_t(waypoints.at(waypoint_idx + 1).coords[0], waypoints.at(waypoint_idx + 1).coords[1], waypoints.at(waypoint_idx + 1).coords[2]);
+    1372             : 
+    1373        1778 :     const double distance_from_segment = distFromSegment(sample, segment_start, segment_end);
+    1374             : 
+    1375        1778 :     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
+    1376             : 
+    1377        1778 :     if (waypoint_idx > 0 || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+    1378             : 
+    1379        1462 :       if (distance_from_segment > max_deviation) {
+    1380         239 :         max_deviation = distance_from_segment;
+    1381             :       }
+    1382             : 
+    1383        1462 :       if (distance_from_segment > _trajectory_max_segment_deviation_) {
+    1384         625 :         segments.at(waypoint_idx) = false;
+    1385         625 :         is_safe                   = false;
+    1386             :       }
+    1387             :     }
+    1388             : 
+    1389        1778 :     if (segment_end_dist < 0.05 && waypoint_idx < (int(waypoints.size()) - 2)) {
+    1390             :       waypoint_idx++;
+    1391             :     }
+    1392             :   }
+    1393             : 
+    1394          26 :   return std::tuple(is_safe, trajectory.size(), segments, max_deviation);
+    1395             : }
+    1396             : 
+    1397             : //}
+    1398             : 
+    1399             : /* getTrajectorySegmentCenterIdxs() //{ */
+    1400             : 
+    1401           0 : std::vector<int> MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+    1402             :                                                                          const std::vector<Waypoint_t>&                    waypoints) {
+    1403             : 
+    1404           0 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs", scope_timer_logger_, scope_timer_enabled_);
+    1405             : 
+    1406             :   // prepare the output
+    1407             : 
+    1408           0 :   std::vector<int> segment_centers;
+    1409             : 
+    1410             :   // index in the path
+    1411           0 :   int last_segment_start = 0;
+    1412             : 
+    1413             :   // index in the trajectory
+    1414           0 :   int last_segment_start_sample = 0;
+    1415             : 
+    1416           0 :   for (size_t i = 0; i < trajectory.size() - 1; i++) {
+    1417             : 
+    1418             :     // the trajectory sample
+    1419           0 :     const vec3_t sample = vec3_t(trajectory[i].position_W[0], trajectory[i].position_W[1], trajectory[i].position_W[2]);
+    1420             : 
+    1421             :     // next sample
+    1422           0 :     const vec3_t next_sample = vec3_t(trajectory[i + 1].position_W[0], trajectory[i + 1].position_W[1], trajectory[i + 1].position_W[2]);
+    1423             : 
+    1424             :     // segment end
+    1425           0 :     const vec3_t segment_end =
+    1426           0 :         vec3_t(waypoints.at(last_segment_start + 1).coords[0], waypoints.at(last_segment_start + 1).coords[1], waypoints.at(last_segment_start + 1).coords[2]);
+    1427             : 
+    1428           0 :     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
+    1429             : 
+    1430           0 :     if (segment_end_dist < 0.05 && last_segment_start < (int(waypoints.size()) - 2)) {
+    1431             : 
+    1432           0 :       last_segment_start++;
+    1433             : 
+    1434           0 :       segment_centers.push_back(int(floor(double(i - last_segment_start_sample) / 2.0)));
+    1435             :     }
+    1436             :   }
+    1437             : 
+    1438           0 :   return segment_centers;
+    1439             : }
+    1440             : 
+    1441             : //}
+    1442             : 
+    1443             : // | --------------------- minor routines --------------------- |
+    1444             : 
+    1445             : /* findTrajectoryAsync() //{ */
+    1446             : 
+    1447          14 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryAsync(const std::vector<Waypoint_t>&  waypoints,
+    1448             :                                                                                                        const mrs_msgs::TrackerCommand& initial_state,
+    1449             :                                                                                                        const double& sampling_dt, const bool& relax_heading) {
+    1450             : 
+    1451          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: starting the async planning task");
+    1452             : 
+    1453          14 :   future_trajectory_result_ =
+    1454          14 :       std::async(std::launch::async, &MrsTrajectoryGeneration::findTrajectory, this, waypoints, initial_state, sampling_dt, relax_heading);
+    1455             : 
+    1456        1710 :   while (ros::ok() && future_trajectory_result_.wait_for(std::chrono::milliseconds(1)) != std::future_status::ready) {
+    1457             : 
+    1458        1696 :     if (overtime()) {
+    1459           0 :       ROS_WARN("[MrsTrajectoryGeneration]: async task planning timeout, breaking");
+    1460          14 :       return {};
+    1461             :     }
+    1462             :   }
+    1463             : 
+    1464          16 :   ROS_DEBUG("[MrsTrajectoryGeneration]: async planning task finished successfully");
+    1465             : 
+    1466          14 :   return future_trajectory_result_.get();
+    1467             : }
+    1468             : 
+    1469             : //}
+    1470             : 
+    1471             : /* distFromSegment() //{ */
+    1472             : 
+    1473        3556 : double MrsTrajectoryGeneration::distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2) {
+    1474             : 
+    1475        3556 :   vec3_t segment_vector = seg2 - seg1;
+    1476        3556 :   double segment_len    = segment_vector.norm();
+    1477             : 
+    1478        3556 :   vec3_t segment_vector_norm = segment_vector;
+    1479        3556 :   segment_vector_norm.normalize();
+    1480             : 
+    1481        3556 :   double point_coordinate = segment_vector_norm.dot(point - seg1);
+    1482             : 
+    1483        3556 :   if (point_coordinate < 0) {
+    1484         124 :     return (point - seg1).norm();
+    1485        3494 :   } else if (point_coordinate > segment_len) {
+    1486        3344 :     return (point - seg2).norm();
+    1487             :   } else {
+    1488             : 
+    1489        3644 :     mat3_t segment_projector = segment_vector_norm * segment_vector_norm.transpose();
+    1490        1822 :     vec3_t projection        = seg1 + segment_projector * (point - seg1);
+    1491             : 
+    1492        3644 :     return (point - projection).norm();
+    1493             :   }
+    1494             : }
+    1495             : 
+    1496             : //}
+    1497             : 
+    1498             : /* getTrajectoryReference() //{ */
+    1499             : 
+    1500           3 : mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+    1501             :                                                                               const ros::Time& stamp, const double& sampling_dt) {
+    1502             : 
+    1503           3 :   mrs_msgs::TrajectoryReference msg;
+    1504             : 
+    1505           3 :   msg.header.frame_id = frame_id_;
+    1506           3 :   msg.header.stamp    = stamp;
+    1507           3 :   msg.fly_now         = fly_now_;
+    1508           3 :   msg.loop            = loop_;
+    1509           3 :   msg.use_heading     = use_heading_;
+    1510           3 :   msg.dt              = sampling_dt;
+    1511             : 
+    1512         418 :   for (size_t it = 0; it < trajectory.size(); it++) {
+    1513             : 
+    1514         415 :     mrs_msgs::Reference point;
+    1515         415 :     point.heading    = 0;
+    1516         415 :     point.position.x = trajectory[it].position_W[0];
+    1517         415 :     point.position.y = trajectory[it].position_W[1];
+    1518         415 :     point.position.z = trajectory[it].position_W[2];
+    1519         415 :     point.heading    = trajectory[it].getYaw();
+    1520             : 
+    1521         415 :     msg.points.push_back(point);
+    1522             :   }
+    1523             : 
+    1524           3 :   return msg;
+    1525             : }
+    1526             : 
+    1527             : //}
+    1528             : 
+    1529             : /* interpolatePoint() //{ */
+    1530             : 
+    1531         189 : Waypoint_t MrsTrajectoryGeneration::interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff) {
+    1532             : 
+    1533         189 :   Waypoint_t      out;
+    1534         189 :   Eigen::Vector4d diff = b.coords - a.coords;
+    1535             : 
+    1536         189 :   out.coords[0] = a.coords[0] + coeff * diff[0];
+    1537         189 :   out.coords[1] = a.coords[1] + coeff * diff[1];
+    1538         189 :   out.coords[2] = a.coords[2] + coeff * diff[2];
+    1539         189 :   out.coords[3] = radians::interp(a.coords[3], b.coords[3], coeff);
+    1540             : 
+    1541         189 :   out.stop_at = false;
+    1542             : 
+    1543         189 :   return out;
+    1544             : }
+    1545             : 
+    1546             : //}
+    1547             : 
+    1548             : /* checkNaN() //{ */
+    1549             : 
+    1550          12 : bool MrsTrajectoryGeneration::checkNaN(const Waypoint_t& a) {
+    1551             : 
+    1552          12 :   if (!std::isfinite(a.coords[0])) {
+    1553           0 :     ROS_ERROR("NaN detected in variable \"a.coords[0]\"!!!");
+    1554           0 :     return false;
+    1555             :   }
+    1556             : 
+    1557          12 :   if (!std::isfinite(a.coords[1])) {
+    1558           0 :     ROS_ERROR("NaN detected in variable \"a.coords[1]\"!!!");
+    1559           0 :     return false;
+    1560             :   }
+    1561             : 
+    1562          12 :   if (!std::isfinite(a.coords[2])) {
+    1563           0 :     ROS_ERROR("NaN detected in variable \"a.coords[2]\"!!!");
+    1564           0 :     return false;
+    1565             :   }
+    1566             : 
+    1567          12 :   if (!std::isfinite(a.coords[3])) {
+    1568           0 :     ROS_ERROR("NaN detected in variable \"a.coords[3]\"!!!");
+    1569           0 :     return false;
+    1570             :   }
+    1571             : 
+    1572             :   return true;
+    1573             : }
+    1574             : 
+    1575             : //}
+    1576             : 
+    1577             : /* trajectorySrv() //{ */
+    1578             : 
+    1579           3 : bool MrsTrajectoryGeneration::trajectorySrv(const mrs_msgs::TrajectoryReference& msg) {
+    1580             : 
+    1581           6 :   mrs_msgs::TrajectoryReferenceSrv srv;
+    1582           3 :   srv.request.trajectory = msg;
+    1583             : 
+    1584           3 :   bool res = service_client_trajectory_reference_.call(srv);
+    1585             : 
+    1586           3 :   if (res) {
+    1587             : 
+    1588           3 :     if (!srv.response.success) {
+    1589           0 :       ROS_WARN("[MrsTrajectoryGeneration]: service call for trajectory_reference returned: '%s'", srv.response.message.c_str());
+    1590             :     }
+    1591             : 
+    1592           3 :     return srv.response.success;
+    1593             : 
+    1594             :   } else {
+    1595             : 
+    1596           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: service call for trajectory_reference failed!");
+    1597             : 
+    1598           0 :     return false;
+    1599             :   }
+    1600             : }
+    1601             : 
+    1602             : //}
+    1603             : 
+    1604             : /* transformTrackerCmd() //{ */
+    1605             : 
+    1606           3 : std::optional<mrs_msgs::Path> MrsTrajectoryGeneration::transformPath(const mrs_msgs::Path& path_in, const std::string& target_frame) {
+    1607             : 
+    1608             :   // if we transform to the current control frame, which is in fact the same frame as the tracker_cmd is in
+    1609           3 :   if (target_frame == path_in.header.frame_id) {
+    1610           3 :     return path_in;
+    1611             :   }
+    1612             : 
+    1613             :   // find the transformation
+    1614           3 :   auto tf = transformer_->getTransform(path_in.header.frame_id, target_frame, path_in.header.stamp);
+    1615             : 
+    1616           0 :   if (!tf) {
+    1617           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: could not find transform from '%s' to '%s' in time %f", path_in.header.frame_id.c_str(), target_frame.c_str(),
+    1618             :               path_in.header.stamp.toSec());
+    1619           0 :     return {};
+    1620             :   }
+    1621             : 
+    1622           0 :   mrs_msgs::Path path_out = path_in;
+    1623             : 
+    1624           0 :   path_out.header.stamp    = tf.value().header.stamp;
+    1625           0 :   path_out.header.frame_id = transformer_->frame_to(tf.value());
+    1626             : 
+    1627           0 :   for (size_t i = 0; i < path_in.points.size(); i++) {
+    1628             : 
+    1629           0 :     mrs_msgs::ReferenceStamped waypoint;
+    1630             : 
+    1631           0 :     waypoint.header    = path_in.header;
+    1632           0 :     waypoint.reference = path_in.points[i];
+    1633             : 
+    1634           0 :     if (auto ret = transformer_->transform(waypoint, tf.value())) {
+    1635             : 
+    1636           0 :       path_out.points[i] = ret.value().reference;
+    1637             : 
+    1638             :     } else {
+    1639           0 :       return {};
+    1640             :     }
+    1641             :   }
+    1642             : 
+    1643             :   //}
+    1644             : 
+    1645           0 :   return path_out;
+    1646             : }
+    1647             : 
+    1648             : //}
+    1649             : 
+    1650             : /* overtime() //{ */
+    1651             : 
+    1652        1753 : bool MrsTrajectoryGeneration::overtime(void) {
+    1653             : 
+    1654        1753 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1655        1753 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1656             : 
+    1657        1753 :   double overtime = (ros::Time::now() - start_time_total).toSec();
+    1658             : 
+    1659        1753 :   if (overtime > (OVERTIME_SAFETY_FACTOR * max_execution_time - OVERTIME_SAFETY_OFFSET)) {
+    1660           1 :     return true;
+    1661             :   }
+    1662             : 
+    1663             :   return false;
+    1664             : }
+    1665             : 
+    1666             : //}
+    1667             : 
+    1668             : /* timeLeft() //{ */
+    1669             : 
+    1670          14 : double MrsTrajectoryGeneration::timeLeft(void) {
+    1671             : 
+    1672          14 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1673          14 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1674             : 
+    1675          14 :   double current_execution_time = (ros::Time::now() - start_time_total).toSec();
+    1676             : 
+    1677          14 :   if (current_execution_time >= max_execution_time) {
+    1678             :     return 0;
+    1679             :   } else {
+    1680          14 :     return max_execution_time - current_execution_time;
+    1681             :   }
+    1682             : }
+    1683             : 
+    1684             : //}
+    1685             : 
+    1686             : // | ------------------------ callbacks ----------------------- |
+    1687             : 
+    1688             : /* callbackPath() //{ */
+    1689             : 
+    1690           1 : void MrsTrajectoryGeneration::callbackPath(const mrs_msgs::PathConstPtr& msg) {
+    1691             : 
+    1692           1 :   if (!is_initialized_) {
+    1693           0 :     return;
+    1694             :   }
+    1695             : 
+    1696             :   /* preconditions //{ */
+    1697             : 
+    1698           1 :   if (!got_constraints_) {
+    1699           0 :     std::stringstream ss;
+    1700           0 :     ss << "missing constraints";
+    1701           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1702           0 :     return;
+    1703             :   }
+    1704             : 
+    1705           1 :   if (!got_tracker_cmd_) {
+    1706           0 :     std::stringstream ss;
+    1707           0 :     ss << "missing position cmd";
+    1708           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1709           0 :     return;
+    1710             :   }
+    1711             : 
+    1712           1 :   if (!got_control_manager_diag_) {
+    1713           0 :     std::stringstream ss;
+    1714           0 :     ss << "missing control manager diagnostics";
+    1715           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1716           0 :     return;
+    1717             :   }
+    1718             : 
+    1719             :   //}
+    1720             : 
+    1721           1 :   {
+    1722           1 :     std::scoped_lock lock(mutex_start_time_total_);
+    1723             : 
+    1724           1 :     start_time_total_ = ros::Time::now();
+    1725             :   }
+    1726             : 
+    1727           1 :   double path_time_offset = 0;
+    1728             : 
+    1729           1 :   if (msg->header.stamp != ros::Time(0)) {
+    1730           0 :     path_time_offset = (msg->header.stamp - ros::Time::now()).toSec();
+    1731             :   }
+    1732             : 
+    1733           1 :   if (path_time_offset > 1e-3) {
+    1734             : 
+    1735           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1736             : 
+    1737           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time);
+    1738             : 
+    1739           0 :     ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1740             :              path_time_offset);
+    1741             :   } else {
+    1742             : 
+    1743           2 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1744             : 
+    1745           1 :     max_execution_time_ = params_.max_time;
+    1746             :   }
+    1747             : 
+    1748           2 :   ROS_INFO("[MrsTrajectoryGeneration]: got path from message");
+    1749             : 
+    1750           1 :   ph_original_path_.publish(msg);
+    1751             : 
+    1752           1 :   if (msg->points.empty()) {
+    1753           0 :     std::stringstream ss;
+    1754           0 :     ss << "received an empty message";
+    1755           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1756           0 :     return;
+    1757             :   }
+    1758             : 
+    1759           2 :   auto transformed_path = transformPath(*msg, "");
+    1760             : 
+    1761           1 :   if (!transformed_path) {
+    1762           0 :     std::stringstream ss;
+    1763           0 :     ss << "could not transform the path to the current control frame";
+    1764           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1765           0 :     return;
+    1766             :   }
+    1767             : 
+    1768           1 :   fly_now_                              = transformed_path->fly_now;
+    1769           1 :   use_heading_                          = transformed_path->use_heading;
+    1770           1 :   frame_id_                             = transformed_path->header.frame_id;
+    1771           1 :   override_constraints_                 = transformed_path->override_constraints;
+    1772           1 :   loop_                                 = transformed_path->loop;
+    1773           1 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1774           1 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1775           1 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1776           1 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1777           1 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1778           1 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1779           1 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1780             : 
+    1781           2 :   std::vector<Waypoint_t> waypoints;
+    1782             : 
+    1783           5 :   for (size_t i = 0; i < transformed_path->points.size(); i++) {
+    1784             : 
+    1785           4 :     double x       = transformed_path->points[i].position.x;
+    1786           4 :     double y       = transformed_path->points[i].position.y;
+    1787           4 :     double z       = transformed_path->points[i].position.z;
+    1788           4 :     double heading = transformed_path->points[i].heading;
+    1789             : 
+    1790           4 :     Waypoint_t wp;
+    1791           4 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    1792           4 :     wp.stop_at = stop_at_waypoints_;
+    1793             : 
+    1794           4 :     if (!checkNaN(wp)) {
+    1795           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    1796           0 :       return;
+    1797             :     }
+    1798             : 
+    1799           4 :     waypoints.push_back(wp);
+    1800             :   }
+    1801             : 
+    1802           1 :   if (loop_) {
+    1803           0 :     waypoints.push_back(waypoints[0]);
+    1804             :   }
+    1805             : 
+    1806           1 :   bool                          success = false;
+    1807           2 :   std::string                   message;
+    1808           2 :   mrs_msgs::TrajectoryReference trajectory;
+    1809             : 
+    1810           1 :   for (int i = 0; i < _n_attempts_; i++) {
+    1811             : 
+    1812           1 :     auto tracker_cmd = mrs_lib::get_mutexed(mutex_tracker_cmd_, tracker_cmd_);
+    1813             : 
+    1814             :     // the last iteration and the fallback sampling is enabled
+    1815           1 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    1816             : 
+    1817           1 :     std::tie(success, message, trajectory) =
+    1818           2 :         optimize(waypoints, transformed_path->header, tracker_cmd, tracker_cmd.full_state_prediction, fallback_sampling, transformed_path->relax_heading);
+    1819             : 
+    1820           1 :     if (success) {
+    1821             :       break;
+    1822             :     } else {
+    1823           0 :       if (i < _n_attempts_) {
+    1824           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    1825             :       } else {
+    1826           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    1827             :       }
+    1828             :     }
+    1829             :   }
+    1830             : 
+    1831           1 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    1832             : 
+    1833           1 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1834             : 
+    1835           1 :   if (total_time > max_execution_time) {
+    1836           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    1837             :               total_time - max_execution_time);
+    1838             :   } else {
+    1839           2 :     ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    1840             :   }
+    1841             : 
+    1842           1 :   trajectory.input_id = transformed_path->input_id;
+    1843             : 
+    1844           1 :   if (success) {
+    1845             : 
+    1846           1 :     bool published = trajectorySrv(trajectory);
+    1847             : 
+    1848           1 :     if (published) {
+    1849             : 
+    1850           2 :       ROS_INFO("[MrsTrajectoryGeneration]: trajectory successfully published");
+    1851             : 
+    1852             :     } else {
+    1853             : 
+    1854           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: could not publish the trajectory");
+    1855             :     }
+    1856             : 
+    1857             :   } else {
+    1858             : 
+    1859           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, no publishing a result");
+    1860             :   }
+    1861             : }
+    1862             : 
+    1863             : //}
+    1864             : 
+    1865             : /* callbackPathSrv() //{ */
+    1866             : 
+    1867           2 : bool MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res) {
+    1868             : 
+    1869           2 :   if (!is_initialized_) {
+    1870             :     return false;
+    1871             :   }
+    1872             : 
+    1873             :   /* mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("callback()"); */
+    1874             : 
+    1875             :   /* precondition //{ */
+    1876             : 
+    1877           2 :   if (!got_constraints_) {
+    1878           0 :     std::stringstream ss;
+    1879           0 :     ss << "missing constraints";
+    1880           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1881             : 
+    1882           0 :     res.message = ss.str();
+    1883           0 :     res.success = false;
+    1884           0 :     return true;
+    1885             :   }
+    1886             : 
+    1887           2 :   if (!got_tracker_cmd_) {
+    1888           0 :     std::stringstream ss;
+    1889           0 :     ss << "missing position cmd";
+    1890           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1891             : 
+    1892           0 :     res.message = ss.str();
+    1893           0 :     res.success = false;
+    1894           0 :     return true;
+    1895             :   }
+    1896             : 
+    1897           2 :   if (!got_control_manager_diag_) {
+    1898           0 :     std::stringstream ss;
+    1899           0 :     ss << "missing control manager diagnostics";
+    1900           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1901             : 
+    1902           0 :     res.message = ss.str();
+    1903           0 :     res.success = false;
+    1904           0 :     return true;
+    1905             :   }
+    1906             : 
+    1907             :   //}
+    1908             : 
+    1909           2 :   {
+    1910           2 :     std::scoped_lock lock(mutex_start_time_total_);
+    1911             : 
+    1912           2 :     start_time_total_ = ros::Time::now();
+    1913             :   }
+    1914             : 
+    1915           2 :   double path_time_offset = 0;
+    1916             : 
+    1917           2 :   if (req.path.header.stamp != ros::Time(0)) {
+    1918           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    1919             :   }
+    1920             : 
+    1921           2 :   if (path_time_offset > 1e-3) {
+    1922             : 
+    1923           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1924             : 
+    1925           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time);
+    1926             : 
+    1927           0 :     ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1928             :              path_time_offset);
+    1929             :   } else {
+    1930             : 
+    1931           4 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1932             : 
+    1933           2 :     max_execution_time_ = params_.max_time;
+    1934             :   }
+    1935             : 
+    1936           4 :   ROS_INFO("[MrsTrajectoryGeneration]: got path from service");
+    1937             : 
+    1938           2 :   ph_original_path_.publish(req.path);
+    1939             : 
+    1940           2 :   if (req.path.points.empty()) {
+    1941           0 :     std::stringstream ss;
+    1942           0 :     ss << "received an empty message";
+    1943           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1944             : 
+    1945           0 :     res.message = ss.str();
+    1946           0 :     res.success = false;
+    1947           0 :     return true;
+    1948             :   }
+    1949             : 
+    1950           4 :   auto transformed_path = transformPath(req.path, "");
+    1951             : 
+    1952           2 :   if (!transformed_path) {
+    1953           0 :     std::stringstream ss;
+    1954           0 :     ss << "could not transform the path to the current control frame";
+    1955           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    1956             : 
+    1957           0 :     res.message = ss.str();
+    1958           0 :     res.success = false;
+    1959           0 :     return true;
+    1960             :   }
+    1961             : 
+    1962           2 :   fly_now_                              = transformed_path->fly_now;
+    1963           2 :   use_heading_                          = transformed_path->use_heading;
+    1964           2 :   frame_id_                             = transformed_path->header.frame_id;
+    1965           2 :   override_constraints_                 = transformed_path->override_constraints;
+    1966           2 :   loop_                                 = transformed_path->loop;
+    1967           2 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1968           2 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1969           2 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1970           2 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1971           2 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1972           2 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1973           2 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1974             : 
+    1975           4 :   std::vector<Waypoint_t> waypoints;
+    1976             : 
+    1977          10 :   for (size_t i = 0; i < req.path.points.size(); i++) {
+    1978             : 
+    1979           8 :     double x       = transformed_path->points[i].position.x;
+    1980           8 :     double y       = transformed_path->points[i].position.y;
+    1981           8 :     double z       = transformed_path->points[i].position.z;
+    1982           8 :     double heading = transformed_path->points[i].heading;
+    1983             : 
+    1984           8 :     Waypoint_t wp;
+    1985           8 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    1986           8 :     wp.stop_at = stop_at_waypoints_;
+    1987             : 
+    1988           8 :     if (!checkNaN(wp)) {
+    1989           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    1990           0 :       res.success = false;
+    1991           0 :       res.message = "invalid path";
+    1992           0 :       return true;
+    1993             :     }
+    1994             : 
+    1995           8 :     waypoints.push_back(wp);
+    1996             :   }
+    1997             : 
+    1998           2 :   if (loop_) {
+    1999           0 :     waypoints.push_back(waypoints[0]);
+    2000             :   }
+    2001             : 
+    2002           2 :   bool                          success = false;
+    2003           4 :   std::string                   message;
+    2004           4 :   mrs_msgs::TrajectoryReference trajectory;
+    2005             : 
+    2006           2 :   for (int i = 0; i < _n_attempts_; i++) {
+    2007             : 
+    2008           2 :     auto tracker_cmd = mrs_lib::get_mutexed(mutex_tracker_cmd_, tracker_cmd_);
+    2009             : 
+    2010             :     // the last iteration and the fallback sampling is enabled
+    2011           2 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2012             : 
+    2013           2 :     std::tie(success, message, trajectory) =
+    2014           4 :         optimize(waypoints, transformed_path->header, tracker_cmd, tracker_cmd.full_state_prediction, fallback_sampling, transformed_path->relax_heading);
+    2015             : 
+    2016           2 :     if (success) {
+    2017             :       break;
+    2018             :     } else {
+    2019           0 :       if (i < _n_attempts_) {
+    2020           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2021             :       } else {
+    2022           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2023             :       }
+    2024             :     }
+    2025             :   }
+    2026             : 
+    2027           2 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2028             : 
+    2029           2 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2030             : 
+    2031           2 :   if (total_time > max_execution_time) {
+    2032           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2033             :               total_time - max_execution_time);
+    2034             :   } else {
+    2035           4 :     ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2036             :   }
+    2037             : 
+    2038           2 :   trajectory.input_id = transformed_path->input_id;
+    2039             : 
+    2040           2 :   if (success) {
+    2041             : 
+    2042           2 :     bool published = trajectorySrv(trajectory);
+    2043             : 
+    2044           2 :     if (published) {
+    2045             : 
+    2046           2 :       res.success = success;
+    2047           2 :       res.message = message;
+    2048             : 
+    2049             :     } else {
+    2050             : 
+    2051           0 :       std::stringstream ss;
+    2052           0 :       ss << "could not publish the trajectory";
+    2053             : 
+    2054           0 :       res.success = false;
+    2055           0 :       res.message = ss.str();
+    2056             : 
+    2057           0 :       ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str());
+    2058             :     }
+    2059             : 
+    2060             :   } else {
+    2061             : 
+    2062           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, not publishing a result");
+    2063             : 
+    2064           0 :     res.success = success;
+    2065           0 :     res.message = message;
+    2066             :   }
+    2067             : 
+    2068           2 :   return true;
+    2069             : }
+    2070             : 
+    2071             : //}
+    2072             : 
+    2073             : /* callbackGetPathSrv() //{ */
+    2074             : 
+    2075           0 : bool MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res) {
+    2076             : 
+    2077           0 :   if (!is_initialized_) {
+    2078             :     return false;
+    2079             :   }
+    2080             : 
+    2081             :   /* precondition //{ */
+    2082             : 
+    2083           0 :   if (!got_constraints_) {
+    2084           0 :     std::stringstream ss;
+    2085           0 :     ss << "missing constraints";
+    2086           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2087             : 
+    2088           0 :     res.message = ss.str();
+    2089           0 :     res.success = false;
+    2090           0 :     return true;
+    2091             :   }
+    2092             : 
+    2093           0 :   if (!got_tracker_cmd_) {
+    2094           0 :     std::stringstream ss;
+    2095           0 :     ss << "missing position cmd";
+    2096           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2097             : 
+    2098           0 :     res.message = ss.str();
+    2099           0 :     res.success = false;
+    2100           0 :     return true;
+    2101             :   }
+    2102             : 
+    2103           0 :   if (!got_control_manager_diag_) {
+    2104           0 :     std::stringstream ss;
+    2105           0 :     ss << "missing control manager diagnostics";
+    2106           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2107             : 
+    2108           0 :     res.message = ss.str();
+    2109           0 :     res.success = false;
+    2110           0 :     return true;
+    2111             :   }
+    2112             : 
+    2113             :   //}
+    2114             : 
+    2115           0 :   {
+    2116           0 :     std::scoped_lock lock(mutex_start_time_total_);
+    2117             : 
+    2118           0 :     start_time_total_ = ros::Time::now();
+    2119             :   }
+    2120             : 
+    2121           0 :   double path_time_offset = 0;
+    2122             : 
+    2123           0 :   if (req.path.header.stamp != ros::Time(0)) {
+    2124           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    2125             :   }
+    2126             : 
+    2127           0 :   if (path_time_offset > 1e-3) {
+    2128             : 
+    2129           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2130             : 
+    2131           0 :     max_execution_time_ = FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset;
+    2132             : 
+    2133           0 :     ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    2134             :              path_time_offset);
+    2135             :   } else {
+    2136             : 
+    2137           0 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    2138             : 
+    2139           0 :     max_execution_time_ = params_.max_time;
+    2140             :   }
+    2141             : 
+    2142           0 :   ROS_INFO("[MrsTrajectoryGeneration]: got path from service");
+    2143             : 
+    2144           0 :   ph_original_path_.publish(req.path);
+    2145             : 
+    2146           0 :   if (req.path.points.empty()) {
+    2147           0 :     std::stringstream ss;
+    2148           0 :     ss << "received an empty message";
+    2149           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str());
+    2150             : 
+    2151           0 :     res.message = ss.str();
+    2152           0 :     res.success = false;
+    2153           0 :     return true;
+    2154             :   }
+    2155             : 
+    2156           0 :   fly_now_                              = req.path.fly_now;
+    2157           0 :   use_heading_                          = req.path.use_heading;
+    2158           0 :   frame_id_                             = req.path.header.frame_id;
+    2159           0 :   override_constraints_                 = req.path.override_constraints;
+    2160           0 :   loop_                                 = req.path.loop;
+    2161           0 :   override_max_velocity_horizontal_     = req.path.override_max_velocity_horizontal;
+    2162           0 :   override_max_velocity_vertical_       = req.path.override_max_velocity_vertical;
+    2163           0 :   override_max_acceleration_horizontal_ = req.path.override_max_acceleration_horizontal;
+    2164           0 :   override_max_acceleration_vertical_   = req.path.override_max_acceleration_vertical;
+    2165           0 :   override_max_jerk_horizontal_         = req.path.override_max_jerk_horizontal;
+    2166           0 :   override_max_jerk_vertical_           = req.path.override_max_jerk_horizontal;
+    2167           0 :   stop_at_waypoints_                    = req.path.stop_at_waypoints;
+    2168             : 
+    2169           0 :   std::vector<Waypoint_t> waypoints;
+    2170             : 
+    2171           0 :   for (size_t i = 0; i < req.path.points.size(); i++) {
+    2172             : 
+    2173           0 :     double x       = req.path.points[i].position.x;
+    2174           0 :     double y       = req.path.points[i].position.y;
+    2175           0 :     double z       = req.path.points[i].position.z;
+    2176           0 :     double heading = req.path.points[i].heading;
+    2177             : 
+    2178           0 :     Waypoint_t wp;
+    2179           0 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    2180           0 :     wp.stop_at = stop_at_waypoints_;
+    2181             : 
+    2182           0 :     if (!checkNaN(wp)) {
+    2183           0 :       ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    2184           0 :       res.success = false;
+    2185           0 :       res.message = "invalid path";
+    2186           0 :       return true;
+    2187             :     }
+    2188             : 
+    2189           0 :     waypoints.push_back(wp);
+    2190             :   }
+    2191             : 
+    2192           0 :   if (loop_) {
+    2193           0 :     waypoints.push_back(waypoints[0]);
+    2194             :   }
+    2195             : 
+    2196           0 :   bool                          success = false;
+    2197           0 :   std::string                   message;
+    2198           0 :   mrs_msgs::TrajectoryReference trajectory;
+    2199             : 
+    2200           0 :   for (int i = 0; i < _n_attempts_; i++) {
+    2201             : 
+    2202           0 :     auto tracker_cmd = mrs_lib::get_mutexed(mutex_tracker_cmd_, tracker_cmd_);
+    2203             : 
+    2204             :     // the last iteration and the fallback sampling is enabled
+    2205           0 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2206             : 
+    2207           0 :     std::tie(success, message, trajectory) =
+    2208           0 :         optimize(waypoints, req.path.header, tracker_cmd, tracker_cmd.full_state_prediction, fallback_sampling, req.path.relax_heading);
+    2209             : 
+    2210           0 :     if (success) {
+    2211             :       break;
+    2212             :     } else {
+    2213           0 :       if (i < _n_attempts_) {
+    2214           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2215             :       } else {
+    2216           0 :         ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2217             :       }
+    2218             :     }
+    2219             :   }
+    2220             : 
+    2221           0 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2222             : 
+    2223           0 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2224             : 
+    2225           0 :   if (total_time > max_execution_time) {
+    2226           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2227             :               total_time - max_execution_time);
+    2228             :   } else {
+    2229           0 :     ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2230             :   }
+    2231             : 
+    2232           0 :   if (success) {
+    2233             : 
+    2234           0 :     res.trajectory = trajectory;
+    2235           0 :     res.success    = success;
+    2236           0 :     res.message    = message;
+    2237             : 
+    2238             :   } else {
+    2239             : 
+    2240           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2241             : 
+    2242           0 :     res.success = success;
+    2243           0 :     res.message = message;
+    2244             :   }
+    2245             : 
+    2246           0 :   return true;
+    2247             : }
+    2248             : 
+    2249             : //}
+    2250             : 
+    2251             : /* callbackConstraints() //{ */
+    2252             : 
+    2253        7894 : void MrsTrajectoryGeneration::callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg) {
+    2254             : 
+    2255        7894 :   if (!is_initialized_) {
+    2256             :     return;
+    2257             :   }
+    2258             : 
+    2259        7951 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got constraints");
+    2260             : 
+    2261        7894 :   mrs_lib::set_mutexed(mutex_constraints_, *msg, constraints_);
+    2262             : 
+    2263        7894 :   got_constraints_ = true;
+    2264             : }
+    2265             : 
+    2266             : //}
+    2267             : 
+    2268             : /* callbackTrackerCmd() //{ */
+    2269             : 
+    2270       62198 : void MrsTrajectoryGeneration::callbackTrackerCmd(const mrs_msgs::TrackerCommandConstPtr& msg) {
+    2271             : 
+    2272       62198 :   if (!is_initialized_) {
+    2273             :     return;
+    2274             :   }
+    2275             : 
+    2276       62252 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got position cmd");
+    2277             : 
+    2278       62198 :   got_tracker_cmd_ = true;
+    2279             : 
+    2280       62198 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    2281             : 
+    2282       62198 :   mrs_lib::set_mutexed(mutex_tracker_cmd_, *msg, tracker_cmd_);
+    2283             : }
+    2284             : 
+    2285             : //}
+    2286             : 
+    2287             : /* callbackControlManagerDiag() //{ */
+    2288             : 
+    2289        9593 : void MrsTrajectoryGeneration::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg) {
+    2290             : 
+    2291        9593 :   if (!is_initialized_) {
+    2292             :     return;
+    2293             :   }
+    2294             : 
+    2295        9650 :   ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got control manager diagnostics");
+    2296             : 
+    2297        9593 :   got_control_manager_diag_ = true;
+    2298             : 
+    2299        9593 :   mrs_lib::set_mutexed(mutex_control_manager_diag_, *msg, control_manager_diag_);
+    2300             : }
+    2301             : 
+    2302             : //}
+    2303             : 
+    2304             : /* //{ callbackDrs() */
+    2305             : 
+    2306          57 : void MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, [[maybe_unused]] uint32_t level) {
+    2307             : 
+    2308         114 :   mrs_lib::set_mutexed(mutex_params_, params, params_);
+    2309             : 
+    2310          57 :   {
+    2311          57 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2312             : 
+    2313          57 :     max_execution_time_ = params.max_time;
+    2314             :   }
+    2315             : 
+    2316         114 :   ROS_INFO("[MrsTrajectoryGeneration]: DRS updated");
+    2317          57 : }
+    2318             : 
+    2319             : //}
+    2320             : 
+    2321             : }  // namespace mrs_uav_trajectory_generation
+    2322             : 
+    2323             : #include <pluginlib/class_list_macros.h>
+    2324             : 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..120582ceef --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html @@ -0,0 +1,601 @@ + + + + + + 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 + + +
+ 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..6e47e448bf53d0552db8b5d387dca11747515e27 GIT binary patch literal 7472 zcmV-09na#4P)(nDt%6ID|%)-Mnilpa`ReMwZGHP=-;{wSq>5NCL1#fS~ns-ARGRIAX(d7{{UV z7?+dAWgy=3XnoXNm95+cQ2PZ`3oV|a6H1C9;ZOR6;GfQFQ74RG;N zPyv=`*i9i&&AGrrr}^xht4+wQe?sX}48E5 zUN1txn>~>wfL9U5>qXY}^+ExPTR6GUObf?X+cdj?<+ZRn{w?&Fjdo)jeGkV6`_*rh z;9HHW5RYId^N_~H{~y(lCK`!vLax1Nb7s(xHn8L_hE~|6-w!m>XTuV9I)bjwd4Xtj z7yT=xpjK$~WYRSDQR$a*MT+WcU`Asz%NJ@(xg~8O$C?1VN-zTc)89`f7tJ;G zs|{S^ogyqsLOgg_`nvU92W_zkhq0<>XyMj8p?SrM!#=tkW0}jtjRCb`8cN0v0BblE$GUGpW%e zWvWse##?s8$@H5AE=O(GFj~QV4MzXvsy*WHo1M+Z`rn@Jh1}C zaB{_A0N32Ui#YmzHpKaqfs9>6fc)(v={lv6rae0qQc@rw`gYDQ!q{|Uhx*Lb>AKe- zpAjY!{(#ld=+Eq+a&*xc4MCMaDUq+jaOMFkU5fz6@8Xl<0EUJ&J|62MX=|Ui`QkR3 zrM6MuAKRApP5Hi+T`%^(up84|!&bnk>3XnFGtkIL$&Xwgjp(p#r;#Fai&Lu&&@#!7 z3@MWodBnKFnZ9N7zEzIP4IZ-Ha$Hlo)+QQ}a59_`Ya>P$Z|{5@v}SsO?WHuFMu&SW;3fsM zG!7dXN2Q|c_tU5u8HF6TaWsaFqgg@nSKa=DogNNw5HK5QzOj>lw0N4h9uiql9+|kK zr<*-BjH!5_NGQO@u^y`or;I@E1&eFVxTXQ)sZaofqLr+|O$u0F*Z2xh!8k&*cNin@ zDPUA5D4nTI(TunWz@MLI){gW4*O&eEpZ&(q4*>irw#&~^1_*%ihQptr&v5Lv2b6Ow z|K_nC0jTOa)kDKS*D#YBnJFmm3P5Ss<1Os>Ac**ZUR?Si+*lh@svs4wKpm&THJZ3b zoeE&~G|}iWp0X*ieGy&@e>Wos%G-&$OXHH;!I<|kg@L1gIV#AG%JHc;{BwZfx z7CjF>7Q+71|JDL@%MiT-f+KQ_F|>#R%b4*idU1{#5)c48{1lDpYPd$O%-G3;AH*#z zxM3~>YUiewWXm4D6-?c&>dcmI4+v0$(Ow*sRf@}4X6 z?*e?}dP)wcVFtF-T)!ImJm7Av!kAvXp^4Ls!|Sh2j`7E6ntQI6dfn8u9Pl_282gQj zOdmBmZos&c1RNRQ5drFiZG zKEq-w0*daM<9RXLUUte5z7oww3QohUL7Nl}nBD($T@&6>Y^~hv)=ZcS%+$3O1G5{r z<$5--R6lyGtwic_G?Id*EvwPwffp7d5UYsC6r=PV%(2p5suDO1TD^vucJ;w`E@&Bu z=XHdzK_rTY>7@W~O8fI{{_7 zf?{_ZzTr}#m&Q;W47$b)uCf873Nr^d<6el2C`?nBkj45VVfcuIK)`%H64qYb?3q!* zFZ!i`!z_HW*Ed`ecnjcrBMS;K!>F4&V1L(aNVN=3pSqLj(!^Lpf7!~1rUQ+uXtpz2 z^WpCh@sQ3I4SD;pOYA_6z++`y)2lBLRGyI|U0fXA4v#o_dTou9V&ihO?p>?!NaxLGtwJY&;!vA7 zfB;xJKR`BM94;0xV&pAdy$6yV|8|Tn!nA1JZ<>##)yi9tj#E(z0j1`0W+&fK@m5~W z>te}I7y#XfY)tS7VCJ29NOnfGBm=GFK9V?+dP0Hekt_`16WMCq2EaS*-43(A1db9% zB|1fdF7zL+du?6oxsF|1qg-3p{0>e<7+f}=o6>=CLsK5)X<{h%4ZMQ2%xrEYdZa#dZ%qKj z81HewDu7~)4&@5f;&eD2M*GiPEH3C^3l=0RtFt8;KC6Et!U77mKC{5aT5MJ=a)SXzjXq-IH{^E4u#qktx`IK30{5 ziSx0hEW|ZaQ5L+w>oh`vmy`t;^;uUIm?3*dwG}$U%un4Gfh&mV~TX8p9vy*&Df5b$yMgOd-xXbIqoy>zxVXiXZiPA2nSnrgIb{xqOy} zKskd6B1JXD0q!=lB`$&8%`p_pA+l)%z?zz(N;#1=oJ;TuA;OhU1@Z9# zd>@ru0DncPyrJ0T``Du9;qnnTtuJncsKR-k0Dew(!K4IZM)GRCXLy^NvP_x-17H)z zM0_y0r8!^0Afg=UY9A6+f5_5^0Uwk$3}|XhLzoiFl_EW@dKMtN&v5=>rIRGVNCOVP zAzjMh7F~;rYSHW5uoS|#=zx0u6bq?NqVtt8RWeN7`j7M)6%ZFLTH=TNkY*`q*dBtZJ8`xmk3baH5KZW zw>B*$;3Wa?k3tY00KA|=^~r1NiTOjiqX_jGbCF7u|C!qdx<_z&y8{#`?BX!nPoQyX zDDVOv_=s1xaY(T~}oJn2h z5idpdwqPVT=XFcm_}v(@HX9>3+-pzQRgZX)x;h|IS4N8`4Phn3(@-AM`97oP)l|^1 zu@q}lNc9KIN9SUzW>qV;<|%6iM@BG!KR*>q95^(G0v`WBL69(Oc{$Fw7lIt0m}}ct z9y5)%q9XYbVET7DdxTuOwDyo0?lqQlT_74ynKH+5u3oc%QX20ZCNbax&k%jr0HGN0 zK~2kku5;bY6UJSRR=vQ{{ki}s#Te?K<(bMz%ZDDHe8<@(fY58X7cpX5id`R#UMWXS z{m15Dj)mk3}ceUJrZi;olL0P zuU&G%X20Rxru}V~wYu*(jIN{HNQeQwaps!8$e6Mt1(XgoL!*Y7OpX*FbN;5&nR&Jl09Eq4CImMn zr+JnNnRZAA-~qYB2JGR13Ol%LQW$YYXJ+1xfNz@NZkC699DY7V!1du?z7V!>sR)&t zLScCqwbgQ|D8>|Qn}1&+XC`IrL2{k196Lr`eg#s8odNiz@k=8|<3RIlu-I1sT_Bf0 zb)8gKp%Pc5-zI( zUn($ubd?ND7t2t zuX0}01!_RS1rlCephhq$gbkmrK)~U9ZXCV>A0%+KYbe{!!rv?M0%Yq3{+`)Vrs zL=A=+w>v*6CL>3M5w~!AYtD=N#Vj@k(e#v)KNh(!#rO^JN0YT9#9z|dOZ|Og zUH^2f>x|G#tgHk5ht`^l5rZYb_F5!d&s>4*a>O7Hm|hqfXoT2o7xt@-7*rHZmaiFM z1atxLf%c7ty{8fZO#(jX&0gDeB?3AERwAHh?2qChe8gi65%mp}G~)S_fs(mj1N9Y? z2I|ZRY5W(u2B^N_q5i`+JTzzJ4}^!V{>$o&TxXL3mWD8=aYlG3(R%zd-h@;wkV)NL zP2uaUdG85vQ$6rT#2!wt0Z^djrZ6IgIPFV(JP{VG~~O!kq1dotsb*(9hjLozw9YYqrs76!xHUm#rGW z)Wp3xs-3tlAXi{ZfmSYXhlDXD`mW93sJiAH=c<#-cdg`_$9R58De{CudRxtkIp-kK zr4eH~h5YW@W-TOM`fK7EU1tJ`spx^g>v2+Z-`%5D;+D_|AQ`Stz({8SP|D1NGJ0UQ z3;q=RZj}w(mF31OXq;J$2!7ha7f1bPJwFlk*3OvRaHV6+1^&mP>)!d+j=bx5)jOyX zaG@#U?1$@SF{sIt%5<%x(Z-t9x{e}w-JE%hEnSz@0NC}@j7@LX7UN+a?`s)Xrv@0sS&F^?ncMJ0(<_>i}utGYzKT8iEvrm|vg7NLSurRk8n*_dX1^)s7#BVF* zfDK(gkpGkdM2*GR(sg=WhnMoE~&>N#r8<3%uP;8j1p$V4ZB@PBcN#j z(B;Hzm^1&{n=)<>+dWCf|FGL-LaJ@Q@1x1>;`?UDS~!Hjkz+Zj0V+4d6XuWRYsbV~+_FcLR-F{n4{_>eK-@HZnZJ*qpG`3%KEc{2_Trr2(84 zab5^#ju<~nlN$A+uAL7Lx;gL8rt_xw5w1pP#06t>k(rG}z3puwa?^c$J76NgE%K#h zhI5TTKn=z}wiJdvEPXkb0<{kO8i3hXa)Bop`3In9_c2)~UVL1u@ zt)bnO$8t^K(vY?^>a(Bg;V&_N92*P|OlLAhJakXLzkB=#=_Bw^;0^0C#`b@~KPpE`@w?9d>m+ zecj}+xDrr)aS6H}t^pt=<)G`Ac;5AC_W5-gp1}kgBr%4NFhw3#>{7gNxaFbK%89C zQvsyKZg=LqIlm17`D_kV4UKRLXZ|?X8kn(}O}G?Fjy-4qb&L}<0II?r(LP|cPMP7d zDgIG2JVTnGZ#4n}MVHJ&DmsN6pspksEuf^7)!*_BW9KlAbn6Lq8@FmWGY(E<(Usc* z16=|%N{S>seb%!HBh6uOaTP1s=qu#!ydvxYDUCJio~}^-Zwh>}c#h(uZJ#Bc(!yyJ z&;Dd*(MO19{N5}+XF#k1-XMu0I$Dh2OeX;+g&>5^OEgNEfvH*Eghl~Ub%v7+gITi& zr^OWv=VGn_Gs)uVE}G1wRC%HGl{mKhz#c7edoXjCtSDaK!%gw*;)(??wev9xIHOnB zyDCbq;k9x2_??uQ1l@>-y!cu(2vC2{9M*pJS3PseovCFwv(l7r+BD4_YM4BwH*2P}uTf=u9e)ZWU&=ds3QJ8>!2+zj})ydL$G^ z5TF95p(u{61sZGmtWD1xfviJ9KDYu4IyZcNmHkVpwd(r48tJ9kfEO|J2BQeZxhJa;^az z!+pa=?=a~bro6*n-|*Kr4F0X2kAK-W{9r6u%{k(w{K7CF-tQaM&G5f*#L;f)+%f&v z+|oih{9C4hg*BVOMiF<>F{JtQJlQ}Vt(cGnp3e0}T= z=hw%Mm;rQueeCI$^7XM7+SkYa^|5mT|DXEU>57VtfU>TqI^&C1cDIo}pW&9iGfP3T zlLd?o`s0WQ#R%1VJtyAn!4NQ!F2XvI?J66m+EHKKCb(O;ffEzQETH~N79oHKip-=-$8pA8WCP9rYi$Hh^|S%Cle(`03ITY2gNl3c#z^r02zV1UvAp~js!CX zlw&08;~LX(BSy1kUZ2vMvblmZ2k`k!%h?h%YM6P}Ysxyn1629(+aQB+XQ^wT(IYE* zbN;6uam;k7>q&~H$iI0scQ-=Y@%je(?X9xa-_$rpK7Kkr=7P^!(ay|G>f^+>&xK+KEd@`jIE^C@^*x4oId^XKr&BjpD}SHs{QUC$13g uK4Icsm+YCAo8-YIzIp2xY%HLlE&l;*1)V9KLf7B`0000a}mb? 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